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ABSTRACT 

A  sequential  extended  Kalman  -filter  and  optimal 
smoothing  algorithm  was  developed  to  provide  real  time 
estimates  o-f  torpedo  position  and  depth  on  the  three 
dimensional  underwater  tracking  range  at  the  Naval  Torpedo 
Station,  Keyport ,  Washington.  The  measurements  consisted  o-f 
acoustic  pulse  transit  times  -from  the  torpedo  to  receiving 
array,  which  are  nonlinear  -functions  o-f  the  positions  and 
the  depth  o-f  the  torpedo,  were  linearized  and  -filter  gains 
and  -filtered  estimates  o-f  states  calculated.  By  running  the 
smoothing  subroutine,  all  past  -filtered  estimates  o-f  states 
and  error  covariance  were  smoothed.  The  program  was  tested, 
using  simulated  torpedo  trajectories  that  traversed  both 
single  and  multiple  arrays,  on  an  IBM-PC.  The  results  showed 
that  -filter  performance  was  dependent  on  system  noise  and 
the  distance  to  the  hydrophone  array  -from  the  torpedo  and 
the  smoothed  estimates  of  states  and  error  covariances  were 
better  than  or  equal  to  the  filtered  estimates. 
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I.   INTRODUCTION 

The  Naval  Torpedo  Station  at  Keyport ,  Washington 
currently  operates  two  three— dimensional  underwater  tracking 
ranges  utilizing  a  sonar  transmitter  installed  in  the 
torpedo  to  be  tracked.  The  transmitter  is  synchronized  with 
a  master  clock.  Timed  acoustic  pulses  are  received  by 
hydrophone  arrays  and  then  relayed  via  cable  to  a  computer 
at  the  observation  site.  The  computer  calculates  the 
positional  coordinates  of  the  torpedo  and  plots  its 
trajectory  through  the  water. 

The  measured  data,  which  consist  of  the  elapsed  time 
•from  transmission  o-f  a"  pulse  until  its  receipt  at  the 
hydrophone  array,  is  corrupted  with  noise  due  to  combined 
e-f-fects  o-f  environmental  -factors  and  measurement 
instruments. 

The  intention  is  to  implement  and  test  a  sequential 
extended  Kalman  filter  and  smoothing  routine  which  processes 
the  transit  times  of  the  acoustic  pulses  and  generates  the 
-filtered  and  smoothed  estimates  of  the  positions  of  tracked 
torpedo  at  a  particular  time.  The  design  takes  into  account 
the  elimination  of  the  storage  problem. 
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1 1 .   DESCRIPTION  OF  RANGE  TRACKING  GEOMETRY 

The  hydrophone  array,  consisting  o-f  -four  independent 
elements,  de-fines  an  orthogonal  coordinate  system  in  which 
transit  time  measurements  are  made.  As  shown  in  Figure  2.1, 
■four  hydrophones  X,  Y,  Z,  and  C  are  on  -four  adjacent 
vertices  separated  by  a  distance  d,  along  the  edge  o-f  the 
cube.  The  origin  o-f  the  array  coordinates  is  at  the  center 
o-f  the  cube  with  the  orthogonal  coordinates  parallel  to  its 
edge.  Positional  information  is  computed  -from  the  transit 
times  o-f  a  periodic  synchronous  acoustic  signal  traveling 
•from  the  torpedo  to  the  -four  hydrophones  on  the  array.  The 
.torpedoes  are  equipped  with  sonar  transmitters  which  are 
transmitting  an  acoustic  signal  in  every  1.31  seconds, 
within  a  range  accuracy  3  to  30  -ft.  When  tracking  by 
multiple  arrays,  the  signal  -from  the  closest  hydrophone 
array  is  de-fined  as  the  basis  -for  the  time  measurements  and 
for  the  range  calculations.  A  more  detailed  description  o-f 
the  range  tracking  capability  is  described  in  CRe-f.  1,  23. 
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Figure  2.1   Geometry  o-f  a  Tracking  Array 
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III.  THEORY 

A.   EXTENDED  KALMAN  FILTER 

The   basic   idea   o-f   the   extended   Kalrnan  -filter  is  to 

relinearize   about   each   estimate   X(k/k),  once  it  has  been 

computed.  As  soon  as  a  new  state  estimate  is  made,  a  new  and 

better   re-ference   state  trajectory  is  incorporated  into  the 

estimation  process.  CRef.  3,  4,  53 

For    the    three— dimensional    location   problem   three 

position   states  (X,  Y,  Z)  and  two  velocity  states  (V   ,  v"  ) 
r-  *   7  ^  x     y 

speci-fy   target   motion.   The   discrete  linear  and  nonlinear 
observation  equations  are    given  by 

X(k-t-l)  =  5  .  X(k)  +  p  .  W(k)  (3.1) 

and 

Z(k)  =  h<X(k),  k)  +  V(k)  (3.2) 

where:   j£  and  p  are  constant  matrices; 

h  is  a  nonlinear  -function  o-f  the  state  X 

W(k)  is  the  plant  excitation  noise; 

V ( k )  is  the  measurement  noise. 

In  these  equations  the  plant  noise  and  measurement  noise  are 
assumed  uncorrelated  (white)  with  zero    mean.  That  is, 

ECW(k) .WT( j) 1    =  Q" (k)  &  . 

K  J 
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and 

ECV(k>  .  VT(  j)  ]  =  R(k)  s, 
-  °k  j 

where:    s  =  * »   k  =  j; 

=  O,   k  £    j. 

In   order   to   apply   the  linear  -filter,  Equation  3.2  is 

expanded   in   a  Taylor  series  about  the  best  estimate  o-f  the 

state   at  that  time  and  only  the  first  order  terms  are  kept. 

Equation  3.2  gives 

Z(k)=H(k).X(k)+V(k)  (3.3) 


where 


H(k)  =  — -- 


ax' 


(3.4) 


X  <k)  =  X  (k/k  -  1) 


X(k/k   -   1)   is   a   predicted  value  o-f  the  state  at  time  k, 

given  the  measurements  until  time  k— 1. 
A  state  error  vector  is  de-fined  by 

X  (k/k)  =  X  (k/k)  -  X (k)  , 
and  a  predicted  state  error  vector  is  de-fined  by 

X(k/k  -  1)  =  X(k/k  -  1)  -  X(k). 
The  covariance  o-f  state  error  matrix  is  de-fined  by 

P(k/k)  =  ECX(k/k).X  T(k/k)3, 
the  predicted  covariance  o-f  state  error  matrix  is  given  by 

P(k/k  -  1)  =  ECX(k/k  -  1).X  T(k/k  -  1)1. 
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The  state  excitation  matrix  is  given  by 

G(k)  =  p.ECW(k) . WT (k) 3. pT , 
and  the  measurement  noise  covariance  matrix  is 

R(k)  =  ECV(k) .VT(k) 3. 
The  Kalman  -filter  equations  are  given  by  CRe-f.  3,  4,  53: 

P(k-t-l/k)  =  $  P(k/k)  $T  +  Q(k)  (3.5) 

G(k)=P(k/k-l)HT(k) CH(k)P(k/k-l)HT(k)+R(k) 3_1    (3.6) 
P(k/k)  =  CI  -  G(k)  H<k)3  P(k/k-l)  (3.7) 


X(k+l/k)  =  $  X<k/k)  (3.S) 

Z(k/k-l)  =  h<X(k/k-l),  k)  (3.9) 

X(k/k)  =  X((k/k-l)  +  B(k)  CZ(k)  -  Z(k/k-l)3    (3.10) 

The  Q  matrix  serves  not  only  to  allow  for  maneuvering 
but  also  to  account  -For  any  model  inaccuracies,  that  is,  any 
discrepancies  between  the  true  action  o-f  the  torpedo  and  its 
characterization  by  Equation  3.1.  The  Q  matrix  also  serves 
to  prevent  the  gain  matrix  G(k)  -from  approaching  zero  by 
always  insuring  uncertanity  in  the  predicted  covariance  o-f 
error  matrix  P(k  +  l/k)  CRe-f.  1,  3,  4,  53. 
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B.   OPTIMAL  SMOOTHING 

Smoothing  is  a  non-real  time  data  processing  scheme  that 
uses  all  measurements  between  0  and  N  to  estimate  the  state 
o-f  a  system  at  certain  time  k,  where  0  .£  k  .<  N.  The  smoothed 
estimate   o-f   X(k)  based  on  all  measurements  between  0  and  N 

is   denoted   by   X(k/N).   The   smoothed   error  covariance  is 

denoted  by  P(k/N)  and  P(k/N)  s<  P(k/k)  means  that  the 
smoothed   estimate   o-f  Xik)        is   at   least   as   good  as  the 

-filtered  estimate  or  equal  to  its  -filtered  estimate  tor  all 
the  time,  except  the  terminal  time.  This  is  shown 
graphically  in  Figure  3.1.  As  portrayed  in  Figure  3.2,  there 
are  three  classes  o-f  particular  interest  because  o-f  their 
applicability  to  realistic  problems  CRe-f.  3,  4,  53.  One  is 
the  Rauch-Tung— Stri  ebel  -form,  which  was  chosen  in  our 
particular  problem  CRe-f.  6,  73. 

The  smoothed  state  estimate  and  the  smoothed  error 
covariance  matrix  are    given  by 

X(k/N)  =  X(k/k)  +  A(k) CX (k+l/N)  -  X(k  +  l/k)II    (3.11) 
X(k+l/k)  =  I  X(k/k)  (3.12) 

P(k/N)=P(k/k)+A(k) CP (k+l/N) -P(k+l/k) HA (k)T     (3.  13) 


when 


A(k)  =  P(k/k)  $T   P  1(k  +  l/k)  for  k  ,<  N, 
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Figure  3.1   Advantage  o-f  Per-forming  Optimal  Smoothing 
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Figure    3.2       Three    types    o-f    smoothing:      (a)     fixed-interval, 
(b)     -fixed-point,     (c)     -fixed-lag    smoothing. 


IV.  PROBLEM  DEFINITION 

A.   OBSERVATION  AND  PLANT  STATE  EQUATIONS 

In    the    torpedo    tracking   problem,   the   non-linear 

observation  equations  are    the  -four  independent  transit  times 

from   the  tracked  t°rpedo  to  the  hydrophones,  T  ,  T  ,  T   and 

c  x  y 

T    .     Thus    the    non— linear    measurement    matrix    is    de-fined    by 

z 

Z(k)     =    CT     (k)        T     <k)        T     <k)        T     (k)3T    +    V<k)  (4.1) 

-  c  x  y  z  - 


where 


T     <k)=---C  <X<k)+d/2>2  +(Y(k)-<-d/2)2  +<Z<k)+d/2)2     ]1/2 
c  vel 


T     <k)=--rC  <X(k)-d/2)2 -»-<Y(k>-<-d/2)2+<Z<k)+d/2)2     I1  /2 
x  vel 


T     <k)=--rC  (X(k)+d/2)2 +(Y(k)-d/2)2 +<Z(k)+d/2)2     I1  /?' 
y  vel 


T  (k)=--rC  (X(k)-«-d/2)2 +<Y<k)+d/2)2 +<Z(k)-d/2)2  ]1/2 
z     vel 


Since  the  transit  times  are  readily  available  and 
non-linear  -functions  o-f  position,  these  equations  can  be 
linearized  and  Kalman  -filter  theory  applied  using  the 
extended   Kalman  -filter.  This  procedure  produces  a  real  time 
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■filtering   on   the   transit  times  T  ,  T  ,  T   and  T  ,  without 

c    x    y       z 

the  necessity  o-f  converting  these  times  to  positions. 

Equation    3.4   can   be   used   to   give   the   linearized 

observation   matrix.   When   the   derivatives   are   taken  and 

evaluated  at  the  predicted  state  values  X(k/k-l)  =  X"  (k)  the 

result  is 


j  X"  (k)  +  d/2   _   Y"  (k)  j-  d/2   n   Z'  (k)  +  d/2  J 


H(k)= 

vel 


denl 

X^_(k2_-_d/2 
den2 

XliJ<i_±_d/2 
den3 


den  1 


den  1 


Y_^j(k_)__+_d/2      Z^__(k2_+_d/2  J 
den2  '  den2  '     ! 


-,       Y"  (k)  -  d/2   „   Z*  (k)  +  d/2  J 

0   — --r — 0 — , 

deno  deno 


X*  (k)  +  d/2   rt   Y"  (k)  +  d/2   rt  ■  Z"  (k)  -  d/2  j 


den4 


den4 


den4 


where: 


denl=C (X*  (k)+d/2)2  + (Y*  ( k ) +d/2) 2  + < Z'  < k ) +d/2) 2  I1"' 


den2=C (X" (k) -d/2)2  + ( Y" ( k ) +d/2) 2  + < Z*  < k ) +d/2) 2  J*  2 


den3=C (X*  (k)+d/2)2  + ( Y" ( k ) -d/2) 2  + < Z"  < k ) +d/2) 2  H* /2 


den4=C <X' (k)+d/2)2  + ( Y" ( k ) +d/2) 2  + ( Z" ( k ) -d/2) 2  3*  7 
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The   measurement   noises,   V(k) 's,   are   assumed   to   be 
era— mean  and  independent  with  a  covariance  matrix 
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The  plant  state  equations  are 


X<k+1>   ;    i  X(k)  +TV  (k)  +  g. 

x         1 


i 


|  V  <k+l>  }    }  V  (k)  +  g  } 

■    X  i      i    x  _  i 


'   —        I       V  t  L,  \        J.   T   U   (  L,  \        J.  r-,  ' 


i  Y(k+1)    j  =  {  Y(k)  +  T  V  <k>  +  g^  j  (4.2) 


■ 


I  V  (k+t)  ]     )  V  (k)  +  g„  ) 

!   y        !     :   y       y4  ! 


{  Z(k+1)    {     {  Z(k)  +  g^  { 


where   X(k),   Y(k>   and  Z(k)  are    the  position  coordinates  o-f 

the   torpedo   at   time  t(k),  V  (k)  and  V  (k)  are  the  X  and  Y 

T  x            y 

components  of  the  velocity. 
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The   excitation  terms  g   through  g   are  included  to  take 

into   account   the   random   changes   in  speed  (y   ),  heading 

t 

<y   '  i  and    depth  ( y_ )  ,  which  are    assumed  to  be  independent, 
9t  L 

zero   mean,  rates  o-f  changes.  Typical  maneuvering  parameters 
■for  the  torpedo  are    given  in  [Re-f.  31. 


2         2 
-  «  -,=«=v.;,  <r.   -ECy   3 

9t  9t  •      9t 


2  2  2 

s*.    =  36  -ft /sec  ;  j*   =  ECy   ] 

t  t        t 


2        2 
ff-=  1  -ft  /  sec;  ff-  =  ECy  1 


The   e-f-fect   o-f  this  excitation  is  to  increase  the  predicted 
covariance  o-f  the  state  error  matrix. 

The  excitation  covariance  matrix  is  given  by 


Q  =  r-EEW(k)  WT(k)3.pT  (4.3) 


and 


v" 

2  x     2    2        2    2 


V 

2   -  <       Y.   >2   2   *  u2   2 

t        t         t 


2 

t  . 


where  the  states  are  evaluated  at  the  current  state 
estimates  X(k/k).  Substituting  these  expressions  in  the  Q 
matrix  results  in 


2  3  2  3 

j  T   2     2      T   2       ,I_x2  I  n  . 

!*2  '   *X*     2  'X'  2  }   'X'V*     2  «X"«V"   U  ! 


J  2    2       T^  2 


n  -  i  <li>2    2       f    2 

u  -  i  2      V*       2  ffV 


1  T2   *2 


*<v-       0  | 


.2   2  ; 


{  symmetric  T   s^,' 


A   more   detailed   derivation   o-f   the  excitation  covariance 
matrix  is  given  in  CRef.  31. 

The  excitation  matrix  serves  not  only  to  take  into 
account  the  possibility  of  maneuvering,  but  of  model 
inaccuracies   as   well.   Q  also  used  to  prevent  the  gains  of 


:    t2 

/2 

0 

0! 

!     T 

0 

oi 

r  = 

i  o 

T2/2 

oi 

i  o 

T 

oi 

i  o 

0 

oi 

the  filter  -from  approaching  zero  as  more  and  more  data  is 
processed,  by  insuring  some  uncertainty  in  the  predicted 
state  values  CRe-f.  3,  4,  53. 

In  the  state  -form,  the  plant  state  equation  is 

JCCk+1)  -  $  X(k>  +  p  W(k)  (4.4) 

where: 

!  1   T   0   0   0  ! 

!  o  l  o  o  o  ! 

$  =  i  0   0   1   T   0  !    and 
i  0   0   0   1   0  ! 
i  0   0   0   0   1  i 

B.   DEFINITION  OF  MULTIPLE  ARRAY  TRACKING 

The  coordinate  system  is  de-fined  as  shown  in  Figure  4.1. 
These  72  positions,  an  XYZ  position  -for  each  o-f  4 
hydrophones  in  6  arrays,  are  placed  into  a  6  x  12  matrix 
HYDRO  and  referenced  throughout  the  program. The  torpedo 
position  is  referenced  to  a  central  level  rectangular 
coordinate  system.  The  non— linear  observation  equations 
become 

Z(k)  =  CT  <k>   T  (k)   T  (k)   T  (k)]T  +  v*(k)      (4.5) 
-         c       x       y       z         - 

where 

T  (k)=---C  (X(k)-X.  ,,)2 +(Y(k)-Y  ^)2 +(Z  (k)-Z.  ^)2  I1  /2 
c     vel  lC  lC  lC 
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T     (k)=--7C (X(k)-X. v)2 +<Y<k)-Y. v)2 +<Z(k)-Z. v)2     ]1/2 
x  vel  1 X  1 X  1 X 


T     (k)=--TC  <X  (k>-X  .  w)  2  +  <Y(k)-Y.  x/)  2  +  <Z  <k)-Z.  w)  2     ]l/2 
y  vel  lY  lY  lY 


T     <k)=--rC (X (k)-X. ,)2 +<Y(k)-Y. ,)2 +(Z(k)-Z. 7)2     1% /2 
z  vel  l Z  l Z  l Z 


and    the    subscripted   variables   X,   Y,   and   Z   are   the 
coordinates  of  a  particular  array  being  used. 

The   decision   parameter  used  to  determine  the  switching 
■from   array  to  array  is  a  straight  handoff.  I-f  the  predicted 

x   position-   X,   .  ;1    ,   is  greater  than  3,000  -feet  from  the 

k  +  l/k 

array   in  use,  then  index  is  incremented  and  the  next  row  of 

HYDRO  is  implememted.  This  placed  into  the  program  the  X,  Y, 

and   Z   positions   of  the  hydrophones  in  the  next  array.     The 

handoff   can  easily  be  utilized  in  real  range  operations,  as 

the   transit   times   from  adjacent  arrays  are    present  at  the 

computer  for  a  particular  time  slot. 


28 


Y(-feet) 


farrayf:  ^arrayf:  ^arrayf 


6k 


6 


5 
•■*- 


4 
-■*■- 


-^arrayf 


___.  '     i 


-♦arrayf 


1 

*— 


i : |_  X(-feet) 


6k 


12k 


lSk 


24k 


30k 


>6k 


a  )   Coordinate  System  -for  Multiple  Array  Tracking 


C  HYDRO 


X  HYDRO 


Y  -HYDRO 


Z  HYDRO 


x     y    z 

x    y 

z     x    y    z 

1 

x    y 

z 

•      i 

36000 : 6000 : o 

1 
1 

36030 ! 6000 

\                  i      • 
■       i      i 

o : 36000 : 6030 : o 

1 

1 

36000 ! 6000 

30 

30000 ! 6000  i  0 

30030 ( 6000 

0 ( 30000 ! 6030 ( 0 

30000 ( 6000 

30 

24000! 6000(0 

24030(6000 

0(24000(6030(0 

24000(6000 

30 

1 8000 ! 6000 !o 

18030(6000 

0( 18000(6030(0 

18000(6000 

30 

1 2000 ! 6000 ( 0 

12030(6000 

0( 12000(6030(0 

12000(6000 

30 

6000 ! 6000 ! 0 

6030(6000 

0(  6000(6030(0 

6000 ( 6000 

30 

b  )  Hydrophone  Array  Location  Matrix 


Figure  4.1  Geometry  o-f  Multiple  Array  Tracking 
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C.   SEQUENTIAL  EXTENDED  KALMAN  FILTER 

In   the   sequential   approach,  after  modifying  the  basic 

Kalman   -filter  equations,  calculations  are    performed  on  each 

of   the   -four   independent   transit   times   in  the  -following 

order:   T  ,   T  ,   T    and  T   for  each  1.31  second  time  slot, 
c     x     y        z 

Since   the   four  transit  times  are    independent  and  processed 

sequentially,   the   covariance  of  error  matrix  and  the  state 

vector  .  are   updated   four  times  during  each  time  slot.  Thus 

more   accurate   estimates   of  the  filter  states  are    achived. 

Modification   of   the   filter   equations   for  the  sequential 

approach   circumvented   the   matrix   inversion   in   the  gain 

equation.  An  invalid  transit  time  measurement  will  result  in 

the    filter    ignoring   the   update   information  -for   that 

particular  measurement  only. 

The   estimate  of  the  states  X(k/k),  based  on  one  transit 

.•V 

time  measurement  are  used  as  the  prediction  X(k/k— 1)  for  the 

calcul ationson   the   next   measurements.   Thus  for  the  first 

time   measurement   T   only  the  first  row  of  the  linearized  H 

c 

matrix    is   calculated   and   then   the   first   gain   column 

corresponding  to  the  first  time  measurement  T   is  calculated 

c 

by 

P(k/k-l)    HT 
n      -                        irow 
G.    .  = — ■         (4.6) 

1C°      H.       P(k/k-l)    H       +  R. 
irow  irow      11 
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where   i  =  1  to  4  corresponding  to  the  -four  measured  transit 
times. 

An   estimate   o-f   the   particular   observation   time   is 
calculated   by  using  Equation  3.9  evaluated  at  the  predicted 

state   X(k/k— 1).   The   difference   between   observed  transit 

times   and   the   estimated   transit  times  -forms  the  residual 
which  is  used  in  the  estimate  equation 


X    =  X(k/k-l)  +  G.   ,  [Residual:  (4.7) 

-  l    -  icol 


This   equation   gives  an  estimate  o-f  the  states  based  on  one 
o-f  the  -four  time  measurements. 

The   covariance   o-f   error   is   calculated   based  on  one 
measurement  by 


P.  =  CI  -  G.   .   H.      3  P.  ,  (4.S) 

l  icol    irow     l—l 


where:  I   is  identity  matrix; 

P.     is   the   covariance   matrix  calculated  from  the 
l-l 

previous   transit   time  measurement  or  if  i  =  1, 

the  predicted  error  covariance  P(k/k— 1). 

Editing    erroneous   time   measurement   is   achieved   by 

implementing   a  three  sigma  gate  using  the  covariance  o-f  the 

measurent   noise   (R)   and   the  covariance  of  the  estimation 
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error    P(k/k).The   gate   then   is   written   -for   each   time 
measurement  i  =  1  to  4: 


gate  =  3*  CC (P  .  maximum) / (4860. )2  3  +  R..}1/2    (4.9) 
JJ  ii 


where  j  =  1,  3,  5.  The  gate  expands  or  decreases  depending 
on  the  confidence  level  o-f  the  position  estimate  and  the 
transit  time.  If  the  difference  between  the  actual  transit 
time  received  and  predicted  transit  time  to  a  particular 
hydrophone  exceeds  the  gate,  the  measurement  is  considered 
unacceptable  and  the  -filter  gain  is  set  to  zero  causing  the 
filter   to   ignore   the   data  and  take  the  prediction  of  the 

states  as  the  estimate  X(k/k)  =  X<k/k-l). 

Bounding  the  residual  bias  error  is  achieved  by  making 
comparison  between  the  average  of  the  absolute  value  of  the 
time  residuals  and  the  preset  threshold.  If  the  average  of 
the  time  residuals  exceeds  the  preset  threshold,  Q  is 
calculated  and  added  to  the  last  updated  covariance  of  error 
matrix  P.  Then  filter  reiterates  the  gain,  covariance,  and 
state  estimate  equations  for  the  same  time  slot.  This 
procedure  continues  until  the  average  of  the  time  residuals 
falls  below  the  preset  threshold  at  which  time  an  acceptable 
state  vector  estimate  has  been  obtained  for  the  time  slot. 


D.   OPTIMAL  SMOOTHING  ALGORITHM 

The  smoothing  solution  starts  with  the  -filtered  estimate 
at  the  last  point  and  calculates  backward  point  by  point 
determining  the  smoothed  estimate  as  a  linear  combination  o-f 
the  -filtered  estimate  at  that  point  an.d  the  smoothed 
estimate  at  the  previous  point  CRe-f.  61. 

It  can  be  seen  -from  the  error  covariances  that  the 
■filter  has  reached  a  steady-state  condition  by  the  end  o-f 
the   -forward  sweep.  As  an  example,  let  us  enter  the  backward 

sweep   at   the  end  point  where  k  =  20.  Here  we  have  X (20/20) 

and  P(20/20).  Since  the  -filter  solution  at  this  point  is 
conditioned  on  all  the  measurement  data,  it  is  also  the 
smoothed  estimate  at  k  =  N  =  20.  We  are  now  ready  to  compute 
the  smoothed  estimate  one  step  back  at  k  =  19.  From 
Equations  3.11,  3.12  and  3.13  we  have 


X<19/20)=  X<19/19)+  A(19) CX (20/20)  -  X<20/19)3 
stored  stored 


X(20/19)  =  5  X(19/19) 
stored 


A(19)  =  P(19/19)  iT   P  *  (20/19) 
stored  stored 


P(19/20)  =  P(19/19)  +  A( 19) CP(20/20)  -  P<20/19>3  AT ( 1 9 ) 
stored  stored      stored 


and  to  compute  the  smoothed  estimate  two  step  back  at  k  =  18 


X<18/20)=  X(1S/18)+  A<18) CX (19/20)  -  XC19/18)] 
stored 


X(19/18)  =  $  X(18/1S) 
stored 


A<18)  =  P<18/18)  5T   P  ±   (19/18) 
stored  stored 


P(18/20)  =  P(18/18)  +  A(18) CP(19/20)  -  P(19/18)3  AT(18) 
stored  stored 


This  procedure  continues  until  the  time  k  reaches  to  1 
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V .   SIMULATION  RESULTS 

A.   MULTIPLE  ARRAY  ADAPTIVE  MANEUVERING  RUN 

The  true  trajectory  of  the  torpedo  is  a  straight  line 
with  a  50  -Ft/sec  velocity  toward  the  origin  o-f  hydrophone 
array  parallel  to  X— axis,  drawing  two  tangent  circles  with 
10  deg/sec  turn  rate,  in  the  horizantal  X-Y  plane  through  a 
multiple  array. 

In  the  first  part  o-f  this  run,  the  initial  position  o-f 
the  torpedo  is  3S000  -ft  in  X,  7000  -ft  in  Y,  and  300  ft  in  Z. 
Figures  5.1  and  5.2  depict  the  -filtered  and  smoothed 
estimate  o-f  the  trajectory,  with  zero  initial  velocity 
errors  and  25  -ft  initial  position  errors  in  X  and  Y.  The 
errors  in  the  -filtered  and  the  smoothed  estimate  o-f 
positions  in  X,  Y  and  Z  are  drawn  in  Figures  5.3,  5.4,  5.5, 
5.6,  5.7  and  5.8.  For  the  Kalman  -filter,  errors  ranged 
between  -1.2  and  2.6  -ft  in  X,  -5.9  and  1.9  ft  in  Y,  0.1  and 
2.5  ft  in  Z.  After  smoothing,  the  errors  occured  in  smaller 
range,  which  is,  between  -1.4  and  2.4  ft  in  X,  -5.0  and  1.9 
ft  in  Y,  0.1  and  0.7  ft  in  Z.  The  diagonal  terms  of  the 
filtered  and  smoothed  error  covariance  matrices  are  shown 
pictorially  in  Figures  5.9,  5.10,  5.11,  5.12,  5.13,  5.14, 
5. 15,  5. 16,  5. 17,  5. 18. 
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In  the  second  part  of  this  run,  the  initial  position  o-f 
the  torpedo  is  35000  ft  in  X,  7000  ft  in  Y,  and  300  ft  in  Z. 
The  filtered  and  smoothed  estimate  of  the  trajectory  ars 
drawn  in  Figures  5.19  and  5.20.  Taking  this  different 
initial  geometry  made  the  errors  in  the  position  of  the 
torpedo  to  take  place  in  bigger  values  during  first  time 
slot  of  the  filtering  and  last  time  slot  of  the  smoothing. 
As  seen  in  Figures  5.21,  5.22,  5.23,  5.24,  5.25  and  5.26, 
errors  ranged  between  -16.3  and  1.9  ft  in  X,  —15.1  and  4.6 
ft  in  Y,  —5.0  and  1.0  -ft  in  Z  for  the  Kalman  filter  and  -for 
the  smoothing  this  error  range  is  between  -12.6  and  1.3  -ft 
in  X,  -12.3  and  4.8  ft  in  Y,  -1.9  and  1.0  ft  in  Z.  The 
diagonal  terms  of  the  filtered  and  smoothed  error  covariance 
matrices  displayed  slightly  di-fferent  magnitude,  as  seen  in 
Figures  5.27,  5.29,  5.29,  5.30,  5.31,  5.32,  5.33,  5.34,  5.35 
and  5.36. 

B.   MULTIPLE  ARRAY  ADAPTIVE  STRAIGHT  RUN 

In  this  run,  the  true  trajectory  of  the  torpedo  is  a 
straight  line  with  a  50  ft/sec  velocity  toward  the  origin  of 
hydrophone  array  parallel  to  X-axis  in  the  horizantal  X-Y 
plane  through  a  multiple  array. 

With  the  initial  position  of  the  torpedo  is  38000  ft  in 
X,  7000  ft  in  Y,  and  300  ft  in  Z.  The  filtered  and  smoothed 
estimate   of   the   trajectory,   with   zero   initial  velocity 
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errors  and  25  ft  initial  position  errors  in  X  and  Y,  are 
depicted  in  Figures  5.37  and  5.3S.  Figures  5.39,  5.40,  5.41, 
5.42,  5.43  and  5.44  give  the  errors  in  the  -filtered  and  the 
smoothed  estimate  o-f  positions  in  X,  Y  and  Z.  For  the  Kalman 
■filter,  errors  ranged  between  -1.6  and  2.6  -ft  in  X,  -5.9  and 
4.7  -ft  in  Y,  -0.2  and  2.5  -ft  in  Z.  After  smoothing,  the 
errors  occured  in  smaller  range,  which  is,  between  -1.1  and 
2.4  -ft  in  X,  -5.0  and  1.7  ft  in  Y,  -0.2  and  0.6  ft  in  Z.  The 
diagonal  terms  of  the  filtered  and  smoothed  error  covariance 
matrices  are  shown  pictorially  in  Figures  5.45  through  5.54. 

C.   SINGLE  ARRAY  ADAPTIVE  MANEUVERING  RUN 

The  previous  tests  described  the  filter  and  smoothing 
performance  for  both  straight  and  maneuvering  runs  through 
multiple  array.  Using  the  same  basic  torpedo  trajectories  as 
in  multiple  array,  similar  tests  are  performed  for 
maneuvering  run  through  single  array. During  the  single  array 
tracking,  the  initial  position  of  the  torpedo  is  7500  ft  in 
X,  1300  ft  in  Y  and  0  ft  in  Z,  which  gives  different  initial 
geometry.  The  filtered  and  smoothed  estimates  of  the 
trajectory  and  the  corresponding  position  errors  in  X,  Y  and 
Z  are  pictorially  given  in  Figures  5.55  through  5.62.  For 
the  Kalman  filter,  errors  ranged  between  -1.6  and  3.3  ft  in 
X,  -19.1  and  8.9  ft  in  Y,  -0.3  and  1.6  ft  in  Z.  After 
smoothing,   the   errors   occured  in  smaller  range,  which  is, 


between  -1.1  and  3.1  -ft  in  X,  -17.9  and  5.0  -Ft  in  Y,  -0.1 
and  1.6  -ft  in  Z.  The  diagonal  terms  of  the  -filtered  and 
smoothed  error  covariance  matrices  Are  shown  pictorially  in 
Figures  5.63  through  5.72. 

D.   SINGLE  ARRAY  STRAIGHT  RUN 

The  purpose  o-f  this  last  series  o-f  tests  is  to 
-functionally  demonstrate  the  performance  o-f  the  -filter  and 
smoothing  during  a  straight  run  through  single  array  using 
the  same  initial  torpedo  position  as  in  single  array 
adaptive  maneuvering  run.  The  -filtered  and  smoothed 
estimates  o-f  the  trajectory  and  the  corresponding  position 
errors  in  X,  Y  and  Z  Are  pictorially  given  in  Figures  5.73 
through  5. SO.  For  the  Kalman  filter,  errors  ranged  between 
-1.6  and  3.3  ft  in  X,  -19.1  and  S.9  ft  in  Y,  -0.3  and  0.3  ft 
in  Z.  After  smoothing,  the  errors  occured  in  smaller  range, 
which  is,  between  —0.6  and  3.1  ft  in  X,  -17.9  and  3.5  ft  in 
Y,  -0.2  and  0.7  ft  in  Z.  The  diagonal  terms  of  the  filtered 
and  smoothed  error  covariance  matrices  are  shown  pictorially 
in  Figures  5. SO  through  5.90. 
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VI.   CONCLUSIONS 

The  sequential  extended  Kalman  -filter  and  smoothing 
routine  su-f  -f  i  ciently  generated  the  -Filtered  and  smoothed 
estimates  o-f  the  states,  which  speci-fy  the  motion  o-f  the 
torpedo.  Errors  generated  by  running  the  routine  on  the 
IBM-PC  are  comparable  to  those  given  in  the  previous  search, 
which  was  done  on  a  large  IBM  computer  CRe-f.  ID. 

In    the   smoothing   problem,   computing   the   predicted 

estimates  o-f  the  states,  X<k+l/k),  -from  the  estimates  o-f  the 

states,  X(k/k),  eliminates  the  storage  problem  -for  X(k+l/k). 

In  -future  studies,  an  algorithm  for  computing  P(k/k)  -from 
P<k+l/k+l>  and  hence  eliminating  the  storage  problem  for 
P<k/k),  should  be  invesigated. 

Examining  the  errors  and  their  CDvariances,  it  is 
evident  that  the  uncertainty  in  position  exist  only  in  the 
Y  direction  -for  the  case  where  the  torpedo  is  moving  along 
the  X  axis.  The  results  o-f  the  straight  run  analyses  show 
that  the  propagation  o-f  the  -filtered  error  covariance  is 
dependent  on  the  path  o-f  the  torpedo  with  respect  to 
hydrophone  array.  Upon  observing  the  error  propagation  it  is 
apparent  that  the  position  errors  exhibit  approximately 
equal    oscillations    about    zero    indicating    that   the 
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measurement  noise  is  the  dominant  error  source  driving  the 
-filter. 

The  smoothed  estimates  o-f  the  states  are  at  least  as 
good  as  or  better  than  the  -filtered  estimates.  The  -filter 
performance  was  dependent  on  system  noise  and  the  distance 
■from  the  torpedo  to  the  hydrophone  array.  Errors  get  bigger 
as  the  torpedo  approaches  the  tracking  limit  o-f  the 
hydrophone  array. 

Additional  work  should  be  done  using  trajectories 
generated  -from  actual  torpedo  runs  on  the  Dabob  test  range. 
The  rotation  and  reduction  o-f  the  error  ellipsoids  should 
be  also  included  in  -future  studies. 

The  -filter  should  be  o-f  use  in  range  sa-fety  in  warning 
-for  possible  collisions.  Also  it  may  prove  invaluable  in 
torpedo  recovery  when  there  is  a  mal -function  and  the  torpedo 
is  sometimes  buried  in  many  -feet  o-f  mude. 
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Figure  5.1   Filtered  Estimate  o-f  Trajectory  of  the  Torpedo 
During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.2   Smoothed  Estimate  o-f  Trajectory  o-f  the  Torpedo 
During  a  Maneuvering  Run  through  Multiple  Array 
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MULTIPLE    ARRRT    ADAPTIVE    MHNEUVERING    RUN 
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Figure  5.3   Error  in  Filtered  Estimate  o-f  Position  in  X  o-f 
the  Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.4   Error  in  Smoothed  Estimate  o-f  Position  in  X  o-f 
the  Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.5   Error  in  Filtered  Estimate  o-f  Position  in  Y  of 
the  Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure    5.6       Error    in    Smoothed    Estimate    o-f    Position    in    Y    o-f 
the    Torpedo    During    a    Maneuvering    Run    through    Multiple    Array 
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Figure  5.7   Error  in  Filtered  Estimate  o-f  Position  in  Z  o-f 
the  Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.8   Error  in  Smoothed  Estimate  o-f  Position  in  Z  o-f 
the  Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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MULTIPLE    ARRAY    ADAPTIVE    MANEUVERING    RUN 
FILTERED    ERROR    COVRRIflNCE    P  (K/K)       WITH      NOISE 
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Figure  5.9   Variance  of  Filtered  Position  Error  in  X  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.10   Variance  o-f  Smoothed  Position  Error  in  X  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.11   Variance  of  Filtered  Velocity  Error  in  X  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.12   Variance  o-f  Smoothed  Velocity  Error  in  X  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.13   Variance  o-f  Filtered  Position  Error  in  Y  of  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.14   Variance  o-f  Smoothed  Position  Error  in  Y  o-f  th< 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.15   Variance  o-f  Filtered  Velocity  Error  in  Y  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.16   Variance  o-f  Smoothed  Velocity  Error  in  Y  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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MULTIPLE    ARRAY    ADAPTIVE    MANEUVERING    RUN 
FILTERED    ERROR    COVflRIflNCE    P  (K/K)       WITH      NOISE 
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Figure  5.17   Variance  o-f  Filtered  Position  Error  in  Z  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.13   Variance  o-f  Smoothed  Position  Error  in  Z  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.19   Filtered  Estimate  o-f  Trajectory  o-f  the  Torpedo 
During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.20   Smoothed  Estimate  o-f  Trajectory  o-f  the  Torpedo 
During  a  Maneuvering  Run  through  Multiple  Array 
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ERROR    IN    FILTERED    ESTIMATE  WITH      NOISE 
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Figure  5-21   Error  in  Filtered  Estimate  of  Position  in  X  o-f 
the  Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.22   Error  in  Smoothed  Estimate  o-f  Position  in  X  of 
the  Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.23   Error  in  Filtered  Estimate  of  Position  i  n  Y  o-f 
the  Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.24   Error  in  Smoothed  Estimate  o-f  Position  i  n  Y  o-f 
the  Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.25   Error  in  Filtered  Estimate  o-f  Position  in  Z  o-f 
the  Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.26   Error  in  Smoothed  Estimate  o-f  Position  in  Z  o-f 
the  Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.27   Variance  o-f  Filtered  Position  Error  i  n  X  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.29   Variance  o-f  Smoothed  Position  Error  in  X  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.29   Variance  o-f  Filtered  Velocity  Error  in  X  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.30   Variance  o-f  Smoothed  Velocity  Error  i  n  X  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.31   Variance  of  Filtered  Position  Error  in  Y  of  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.32   Variance  o-f  Smoothed  Position  Error  i  n  Y  of  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.33   Variance  o-f  Filtered  Velocity  Error  in  Y  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.34   Variance  o-f  Smoothed  Velocity  Error  i  n  Y  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.35   Variance  o-f  Filtered  Position  Error  i  n  Z  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.36   Variance  o-f  Smoothed  Position  Error  i  n  Z  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.37   Filtered  Estimate  o-f  Trajectory  o-f  the  Torpedo 
During  a  Straight  Run  through  Multiple  Array 
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Figure  5.38   Smoothed  Estimate  o-f  Trajectory  o-f  the  Torpedo 
During  a  Straight  Run  through  Multiple  Array 
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Figure  5.39   Error  in  Filtered  Estimate  p-f  Position  in  X  o-f 
the  Torpedo  During  a  Straight  Run  through  Multiple  Array 


MULTIPLE      ARRAY      ADAPTIVE 

STRAIGHT    RUN 

ERROR    IN    SMOOTHED    ESTIMATE 

WITH      NOISE 

o 

o 

1—    o 
(\J  - 

ERROR 

0.00 

1 

x    o 

<M 

1 

0 

i               i               i               i 
20            40            60            80 

i             '  i 
100          120 

TIME      SLOTS 

Figure  5.40   Error  in  Smoothed  Estimate  o-f  Position  i  n  X  o-f 
the  Torpedo  During  a  Straight  Run  through  Multiple  Array 
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Figure  5.41   Error  in  Filtered  Estimate  o-f  Position  in  Y  o-f 
the  Torpedo  During  a  Straight  Run  through  Multiple  Array 
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Figure  5.42   Error  in  Smoothed  Estimate  o-f  Position  in  Y  o-f 
the  Torpedo  During  a  Straight  Run  through  Multiple  Array 
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Figure  5.43   Error  in  Filtered  Estimate  of  Position  in  Z  of 
the  Torpedo  During  a  Straight  Run  through  Multiple  Array 


MULTIPLE      ARRAY      ADAPTIVE 

STRAIGHT    RUN 

ERROR    IN    SMOOTHED    ESTIMATE 

WITH      NOISE 

o 
oo 

o  " 

o  " 

ERROR 

0.00 

1 

M   ° 

O 

0 

i               j               i               i 
20            40            60            80 

i                i 
100          120 

TIME      SLOTS 

Figure  5.44   Error  in  Smoothed  Estimate  of  Position  i n  Z  of 
the  Torpedo  During  a  Straight  Run  through  Multiple  Array 
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Figure  5.45   Variance  o-f  Filtered  Position  Error  in  X  of  the 
Torpedo  During  a  Straight  Run  through  Multiple  Array 
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Figure  5.46   Variance  o-f  Smoothed  Position  Error  in  X  o-f  the 
Torpedo  During  a  Straight  Run  through  Multiple  Array 


63 


•      MULTIPLE      ARRAY      ADAPTIVE      STRAIGHT    RUN 

FILTERED    ERROR    COVflRIANCE    P  (K/K)      WITH      NOISE 

o 

o 

o 

_       J  _ 

o    ~ 

_    o 
*     o 

o 

CO  _ 

-,     OJ 

1 

f\J      * 

d 

.     *     o 

1 11 

o 

d  i 

CM  r?     . 

i              A/li  i ' — v 

w     U     o 

„    «'     N 

,-,    UJ    co 

i       A -1'        Kl\      hi         J                     J 

V-     (A) 

1.     U          N     ii\MkA  i  A        i  M    r 

*  > 

\J  A  Jy             1-JIVVVmA  r\A»  .aA  a/  VL^ 

*  r~  ° 

^ul^  •            ^J'Hi  \\n\mW  ' 

u  it  ° 

Vj     M                    >J   ■  ■   1    *■" 

°-  —  o  H 

1                      1                      I                      I                      I                      I 

0              20            40            60            80            100          120 

TIME       SLOTS 

Figure  5.47   Variance  o-f  Filtered  Velocity  Error  i  n  X  o-f  the 
Torpedo  During  a  Straight  Run  through  Multiple  Array 
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Figure  5.48   Variance  o-f  Smoothed  Velocity  Error  in  X  o-f  the 
Torpedo  During  a  Straight  Run  through  Multiple  Array 
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Figure  5.49   Variance  o-f  Filtered  Position  Error  in  Y  o-f  the 
Torpedo  During  a  Straight  Run  through  Multiple  Array 
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Figure  5.50   Variance  o-f  Smoothed  Position  Error  in  Y  o-f  th< 
Torpedo  During  a  Straight  Run  through  Multiole  Array 
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Figure  5.51   Variance  o-f  Filtered  Velocity  Error  in  Y  o-f  the 
Torpedo  During  a  Straight  Run  through  Multiple  Array 
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Figure  5.52   Variance  o-f  Smoothed  Velocity  Error  i  n  Y  o-f  the 
Torpedo  During  a  Straight  Run  through  Multiple  Array 
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Figure  5.53   Variance  o-f  Filtered  Position  Error  in  Z  o-f  the 
Torpedo  During  a  Straight  Run  through  Multiple  Array 
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Figure  5.54   Variance  o-f  Smoothed  Position  Error  in  Z  o-f  the 
Torpedo  During  a  Straight  Run  through  Multiple  Array 
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Figure  5.55   Filtered  Estimate  o-f  Trajectory  of  the  Torpedo 
During  a  Maneuvering  Run  through  Single  Array 


SINGLE    ARRAY      ADAPTIVE      MANEUVERING    RUN 
SMOOTHED    ESTIMATE    OF    TRAJECTORY        WITH      NOISE 


310 


t r 

390  470 

XCK/ND 


550 
(FT) 


630 
*10 


710 


790 


Figure  5.56   Smoothed  Estimate  o-f  Trajectory  o-f  the  Torpedo 
During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.57   Error  in  Filtered  Estimate  o-f  Position  in  X  of 
the  Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.58   Error  in  Smoothed  Estimate  o-f  Position  in  X  o-f 
the  Torpedo  During  a  Maneuvering  Run  through  Single  Array 


69 


SINGLE 

ARRAY      ADAPTIVE      MANEUVERING    RUN 

ERROR    IN 

FILTERED    ESTIMATE                    WITH      NOISE 

o 
a 

o  _ 

(—    o 
U_    o 

o  " 

AfW\ 

T    ERROR 

00     -10.00 

/ 

a  " 

CM 

i     i 
0 

i                i                i                i                i            ■    i 
20            40            60            80            100          120 

TIME      SLOTS 

Figure  5.59   Error  in  Filtered  Estimate  o-f  Position  in  Y  o-f 
the  Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.60   Error  in  Smoothed  Estimate  o-f  Position  in  Y  o-f 
the  Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.61   Error  in  Filtered  Estimate  o-f  Position  in  Z  o-f 
the  Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.62   Error  in  Smoothed  Estimate  o-f  Position  in  Z  o-f 
the  Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.63   Variance  o-f  Filtered  Position  Error  i  n  X  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5-64   Variance  o-f  Smoothed  Position  Error  in  X  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.65   Variance  o-f  Filtered  Velocity  Error  in  X  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Sinlge  Array 
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Figure  5.66   Variance  o-f  Smoothed  Velocity  Error  in  X  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.67   Variance  o-f  Filtered  Position  Error  in  Y  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.68   Variance  o-f  Smoothed  Position  Error  i  n  Y  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.69   Variance  o-f  Filtered  Velocity  Error  in  Y  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.70   Variance  o-f  Smoothed  Velocity  Error  in  Y  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.71   Variance  o-f  Filtered  Position  Error  in  Z  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 


SINGLE    ARRAY      ADAPTIVE      MANEUVERING    RUN 

SMOOTHED    ERROR    COVRRIRNCE    P  IK/NJ      WITH      NOISE 

o 

o 

_.       o 

o    <° 

— t 

\ 

o 

\ 

o  _ 

=1" 

L 

5,5) 

.00 

^a 

w           °  - 
|_|    l—    o 

^-^__ 

°-    —    o  i 

i       i       i       i       i       i 

0               20            40            60            80             100          120 

TIME       SLOTS 

Figure  5.72   Variance  o-f  Smoothed  Position  Error  i  n  Z  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.73   Filtered  Estimate  o-f  Trajectory  o-f  the  Torpedo 
During  a  Straight  Run  through  Single  Array 
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Figure  ,5.74   Smoothed  Estimate  o-f  Trajectory  o-f  the  Torpedo 
During  a  Straight  Run  through  Single  Array 
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SINGLE      ARRAY         ADAPTIVE      STRAIGHT      RUN 
ERROR    IN    FILTERED    ESTIMATE  WITH      NOISE 
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Figure  5.75   Error  in  Filtered  Estimate  o-f  Position  in  X  o-f 
the  Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5.76   Error  in  Smoothed  Estimate  o-f  Position  i  n  X  o-f 
the  Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5.77   Error  in  Filtered  Estimate  o-f  Position  in  Y  o-f 
the  Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5.78   Error  in  Smoothed  Estimate  o-f  Position  i  n  Y  o-f 
the  Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5.79   Error  in  Filtered  Estimate  o-f  Position  in  Z  o-f 
the  Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5.  SO   Error  in  Smoothed  Estimate  o-f  Position  in  Z  o-f 
the  Torpedo  During  a  Straight  Run  through  Single  Array 


30 


SINGLE      ARRAY         ADAPTIVE      STRAIGHT      RUN 

FILTERED    ERROR    CQVflRIANCE    P  (K/K)      WITH      NOISE 

o 
o 

o 

-         <M  _ 

O     "* 

H 

*    o 

o 

O  _ 
CO 

1. 1) 

.00 

KhJ^ 

—           o 

iC     "    o 

Q_     1-        *  - 

^-v^rV^M/^ 

a-    ~    o  n 

i      i      i      i      i      i 

0              20            40            60            80            100          120 

TIME      SLOTS 

Figure  5.81   Variance  of  Filtered  Position  Error  in  X  o-f 
Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5.S2   Variance  o-f  Smoothed  Position  Error  i  n  X  o-f 
Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5.  S3   Variance  o-f  Filtered  Velocity  Error  i  n  X  o-f 
Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5. 34   Variance  o-f  Smoothed  Velocity  Error  in  X  o-f 
Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5.85   Variance  o-f  Filtered  Position  Error  in  Y  o-f  the 
Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5.36   Variance  o-f  Smoothed  Position  Error  in  Y  o-f  the 
Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5.S7   Variance  o-f  Filtered  Velocity  Error  in  Y  o-f 
Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5.S8   Variance  o-f  Smoothed  Velocity  Error  i  n  Y  o-f 
Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5.39   Variance  o-f  Filtered  Position  Error  in  Z  of 
Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5.90   Variance  o+  Smoothed  Position  Error  in  Z  o-f  the 
Torpedo  During  a  Straight  Run  through  Single  Array 
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APPENDIX  A 
PROGRAM  DESCRIPTION 

A.  GENERAL 

The  sequential  extended  Kalman  -filter  and  Smoothing 
routine  is  described  in  detail  by  CRef.  13.  Implementation 
is  done  by  using  F0RTRAN77  compilers  on  IBM-PC.  CRe-f.  10, 
11,  12,  133. 

B.  RUNNING  THE  PROGRAM  ON  THE  IBM-PC 

These  directions  apply  -for  the  IBM-PC  computer  or  other 
computers  (compatibl  es)  with  two  -f  loopy  disk-drives,  640K 
memory,  col or/graphi c  board,  math  coprocessor  and  paralel 
dot  matrix  printer  or  printer/plotter.  The  software  utilized 
during  the  simulation  studies  arsi 

1.  Operating  System  DOS  2.10  with  required  -files  to 
create  virtual  disk  and  -full  screen  editor 
uti 1 i  ties. 

2.  IBM  Professional  FORTRAN  Compiler  1.00. 

3.  Microsoft  F0RTRAN77  3.20. 

4.  Plotworks  PL0TS8.LIB. 

5.  Source  -files. 

Getting    the   sequential   extended   Kal  man   -filter  and 

smoothing    routine   started   is   essentially   a   five  step 

process:   start  your  computer;  edit  the  source  file  and  make 

required   changes   and  then  compile;  run  the  executable  file 
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and  get  the  data  to  be  available  -for  plotting  routine;  edit 
the  source  -file  o-f  plotting  routine  and  make  the  necessary 
changes  -For  plotting  titles  and  then  compile;  run  plotting 
routine.  Start  the  computer  up  with  an  operating  system  and 
get  the  program  running  simply  by  typing  "RUN",  which  is 
given  in  Appendix  E,  at  promt  "A>" . 
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APPENDIX  B 
SEQUENTIAL  EXTENDED  KALMAN  FILTER  AND  OPTIMAL  SMOOTHING 

PROGRAM  LISTING 


PROGRAM  THESIS 

C 

C 

REAL*8  XKKM1 (5) ,PKKM1 (5,5) ,PHI (5,5) , GAMMA (5,3) ,GATE 
REAL*8  GAMMAT(3,5)  ,C0VW(3,3)  ,C0W(4,4)  ,QTEMP(5,3)  ,P 
REAL*8  TRUX(121) ,TRUY(121) ,TRUZ(121) ,ZI (4) ,HR0W(5) 
REAL*8  ZHAT,GDEN0M,GDTEMP,PDUM(5,5) ,PI (5,5) ,WHTN,A14 
REAL*8  ZDIFF(4) ,ZIC<4) ,XI (5) ,XKK(5) ,PKK<5,5) ,ZDIFAV 
REAL*8  DATR(17) , WINIT ,PHIPKK (5 ,5) ,PKTEMP(5,5) ,SIGACC 
REAL*8  SIGDIV,SIGCC,XKERR(121) ,YKERR(121) ,XP6(5,121) 
REAL*8  HYDRO (6, 12) ,XB(4) ,YB(4) ,ZB(4) ,XSERR(121) ,TD(3) 
REAL*8  SIGCCC,SIGAAC,SIGDDI,XP(5,121) ,SMTH(121) ,GI (5) 
REAL*8  P5  (121, 5,  5),  SSI  (121, 5, 5),  PI  (121, 5, 5),  £2(5,  5) 
REAL*8  YSERR(121) ,ZSERR(121) ,GNUM(5) ,PHIT (5, 5) , XP1 (5) 
REAL*8  ZDIFT0,CH(5,5) , TEMPI (5,5) ,XNNM1 (5) ,TEMP2(5) 
REAL*8  AK(5,5) ,AKT(5,5) ,TEMP3(5) ,TEMP4(5,5) ,XKKS(5) 
REAL*8  PNNM1 (5,5) ,TEMP5(5,5) ,TEMP6(5,5) ,PKKS(5,5) 
REAL*8  SS2(5) ,P2(5,5) ,SS3(5,5) ,SS3R(5,5) ,SIG 
REAL*8  ZKERR(121) , X1KERR, X2KERR , Y1KERR , Y2KERR, Z1KERR 
REAL*8  Z2KERR , X 1 SERR , X2SERR , Y 1 SERR , Y2SERR , Z 1 SERR 
REAL*8  Z2SERR 

C  COORDINATES  OF  HYDROPHONE  ARRAY,  FOR  MULTIPLE  ARRAY 

DATA  HYDRO/36000. ,30000. ,24000. , 18000. , 12000. ,6000. 
+   , 6*6000 . , 6*0 . O , 36030 . , 30030 . , 24030 . , 1 8030 . , 1 2030 . 
+   , 6030 . , 6*6000 . , 6*0 . O , 36000 . , 30000 . , 24000 . , 1 8000 . 
+   , 1 2000 . , 6000 . , 6*6030 . , 6*0 . 0 , 36000 . , 30000 . , 24000 . 
+   , 18000. , 12000. ,7*6000. ,6*30. / 

C 

DATA  PKKM1/1000. 0,5*0.0, 1000.0,5*0.0, 1000.0,5*0.0 
+  ,1000.0,5*0.0,1000.0/ 

DATA    PHI/1. ,4*0. ,1.31 , 1. ,5*0. , 1. ,4*0. ,1.31 , 1. ,5*0. 
+  ,  1.  / 

DATA    GAMMA/0.858,1.31 ,5*0.0,0.858, 1.31 ,5*0.0, 1.31/ 
DATA    COVW/ 1.0, 3*0.0, 1 . 0,3*0. 0, 1 . 0/ , WIN IT/0. 49/ 
DATA    COW/  1 .  OD-8  ,  4*0 .0,1.  OD-8  ,  4*0 .0,1.  OD-S  ,  4*0 .  0 
+  ,1.0D-8/ 

C  DATA  FOR  MULTIPLE  ARRAY  TRACKING 

DATA  DATR/38000. ,7000. ,300. ,-50. ,0. ,3*0. ,3*0. 
+,4.712389,. 1745329, . 1 ,8. 1 ,600. ,800. / 
DATA  XKKM 1 /3797S . O , -50 . 0 , 6975 .0,0.0, 300 . 0/ 

C  SECOND  DATA  FOR  MULTIPLE  ARRAY  TRACKING 

C       DATA  DATR/35000. ,7000. ,300. ,-50. ,0. ,3*0. ,3*0. 

C      +,4.712389, . 1745329, . 1 ,8. 1 ,600. ,800. / 

C       DATA  XKKM1/34975.0, -50. 0r6975. 0,0. 0,300.0/ 
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C  FIRST  DATA  FOR  SINGLE  ARRAY  TRACKING 

C       DATA  DATR/7500.0, 1300. 0,0.0,-50. 0,0.0,3*0. 0,3*0. O 

C      +,4.712389,. 1745329,0. 1,8. 1 ,600.0,800.0/ 

C       DATA  XKKM1/7475. 0,-50. 0,1275. 0,0. 0,0.0/ 

C  SECOND  DATA  FOR  SINGLE  ARRAY  TRACKING 

C       DATA  DATR/2000. 0,1000. 0,300. 0,-50. 0,0. 0,3*0. 0,3*0.0 

C      +,4.712389,. 1745329,0. 1 ,8. 1 ,600.0,800.0/ 

C       DATA  XKKM1/1975. 0,-50. 0,975. 0,0. 0,300.0/ 

C  DATA  FOR  SUBROUTINE  QFIND 

DATA  SIGACC/36. 2/ ,SIGDIV/1 . 0/ ,SIGCC/22. 2/ ,NZD IFF/4/ 
DATA  II/l/,IJ/2/,IK/3/,IL/4/,IM/5/,MINE/l/,JTIME/119/ 
C  OPEN  STATEMENTS  FOR  OUTPUT  FILES 
OPEN  <4,FILE= ' TRUDI . DAT ' ) 
OPEN (13,FILE='PKK. DAT' ) 
OPEN ( 1 1 ,FILE= 'PKN. DAT ' ) 
OPEN (7 ,FILE= ' XKK. DAT ' ) 
OPEN (8 ,FILE= ' XKN. DAT ' ) 
OPEN (9 ,FILE= ' XKERR. DAT ' ) 
OPEN ( 10 ,FILE= ' XSERR. DAT ' ) 
OPEN (12,FILE= 'OUTPUT. DAT' ) 
C 

SIGCC  =  (SIGCC  *  3.141592654)  /  180.0 
C  TRANSPOSE  OF  GAMMA  MATRIX 

CALL  TRANS (GAMMA, IM, IK ,GAMMAT) 
C  TRANSPOSE  OF  PHI  MATRIX 

CALL  TRANS (PHI ,IM,IM,PHIT) 
C  USE  THESE  STATEMENTS  TO  CALCULATE  CONSTANT  Q  -  MATRIX 
C       CALL  PROD(GAMMA,COVW,IM,IK,IK,QTEMP) 
C       CALL  PROD(QTEMP,GAMMAT,IM,IK,IM,Q) 

ITIME  -  JTIME  +  1 
C  USE  THIS  STATEMENT  FOR  MULTIPLE  ARRAY  TRACKING 

17  =  O 
C********************************************************** 
C  TIME  SLOTS  START  HERE 

DO  128  KK  =  1  ,  ITIME 
WRITE (*, 562)  KK 
562    FORMAT (/,10X, 'TIME  SLOT  IN  FILTERING  :',I5) 
C 

C&8C&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&S. 
C  CHOSE  THE  HYDROPHONE  ARRAY  FOR  MULTIPLE  ARRAY  TRACKING 
IF(XKKM1 (1) .GE. 33000.0)  18  =  1 

IF( (XKKM1 (1 ) .GE.27O0O.0) .AND. (XKKM1 (1) . LT. 33000.0) ) 
+      18  =  2 

IF( (XKKM1 (1) .GE. 21000.0) .AND. (XKKM1 (1) .LT. 27000.0) ) 
+      18  =  3 

IF( (XKKM1 (1) .GE. 15000.0) .AND. (XKKM1 (1) .LT. 21000.0) ) 
+      18  =  4 

IF( (XKKM1 (1) .GE. 9000.0) .AND. (XKKM1 (1) .LT. 15000.0) ) 
+      18  =  5 

IF(XKKM1 (1) .LT. 9000.0)  18  =  6 


89 


207    DO  205  13  =  1  ,  IL 

14  =  3  *  13 

15  =  14  -  2 

16  =  14  -  1 

XB(I3)  =  HYDR0(IS,I5) 
YB(I3)  =  HYDR0(I8,I6) 
ZB<I3)  =  HYDR0(IS,I4> 
205    CONTINUE 
C  WRITE  THE  COORDINATES  OF  CHOSEN  HYDROPHONE  ARRAY 
IF(I7.NE.ia>  THEN 
WRITE(*,217)  18, KK 
217     FORMAT (/, 1 OX, 'ARRAY  ',12 

+  ,'  STARTS  TRACKING  AT  TIME  ',13) 

WRITE <*, 216)  KK,I8, (I3,XB(I3) ,YB(I3) 
+  -,ZB(I3)  ,13  =  1  ,  IL) 

216     FORMAT (15, I5,4(T11,I5,3(2X,D14.8),/)) 

17  =  18 
END  IF 

C&S^&&&&&&&S<&&&S<&&S<&S<&S<&S<&&&8<&&S<&^ 

PAAAAAAAAAAAAAAAAA  .«*%  •N  .*N A  S\  .+\  ,*\  .'X  S\  S\  j*S  .-S  AAAAAAAA  A/\AA  ,+\  .'*  X\  ,*\  S\  .'•*  ,-N  .A.  ./S,  .-^  .-X  •-.  ,«N  .■'*.  .*•*.  .•*■.  .•>. 

C  CALCULATE  THE  TRUE  TIMES  AND  THE  TRUE  TRAJECTORY 

C  USE  THIS  CALL  STATEMENT  FOR  MULTIPLE  ARRAY  TRACKING 

CALL  TRJC3 ( KK , DATR , Z I , TD , XB , YB , ZB ) 
C  USE  THIS  CALL  STATEMENT  FOR  SINGLE  ARRAY  TRACKING 
C        CALL  TRAJEC(KK,DATR,ZI,TD) 

A14  =  PHI (1,2) 

TRUX(KK)  =  TD(1) 

TRUY(KK)  =  TD(2) 

TRUZ(KK)  =  TD(3> 
C 

WR I TE ( 4 , 306 )  KK , TRUX ( KK ) , TRUY ( KK ) , TRUZ ( KK ) 

pAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA/-. 

C*$***$*f***$***$*f^$*S$****$*******$**J**$$****f$****$*J:** 
C  ROW  OF  H  -  MATRIX 

MINE  =  1 
163    DO  132  IROW  =  1  ,  IL 

NZDIFF  =  4 
C  USE  THIS  CALL  STATEMENT  TO  RUN  NOISE  SUBROUTINE 

CALL  NOISE (WINIT,WHTN) 

WHTN  =  (  1.0  /  3.0  )  *  WHTN 
C  ZERO  NOISE 
C  WHTN  =0.0 

C  USE  THIS  CALL  STATEMENT  FOR  SINGLE  ARRAY  TRACKING 
C  CALL  CHR0W(IR0W,XKKM1 ,HROW) 

C  USE  THIS  CALL  STATEMENT  FOR  MULTIPLE  ARRAY  TRACKING 

CALL  CHR0W3(IR0W,XKKM1 , HROW , XB, YB , ZB) 
C########################################################## 
CGCK]  =  <:PCK/K-1  ] (5x5)  *  HT(5xl)  J  /  i    H(l>:5) 
C  *  P  C  K  /  K  -  1  3(5x5)  *  HT(5xl)  +■  COVC  V  11(1)  J 

CALL  MMULT ( PKKM 1 , HROW , I M , I M , GNUM ) 

CALL  VMULT(HROW,GNUM,IM,GDTEMP) 
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GDENOM  =  GDTEMP  +■  COW  (  I  ROW  ,  I  ROW) 
DO  134  IX  =  1  ,  IM 
GI(IX)  =  GNUM(IX)  /  GDENOM 

134  CONTINUE 
C########################################################## 

CPCK/K3    =    t     1(5x5)     -    G    C    K    3(5x1)     *    H     (1x5)     3- 
C  *    P    C    K    /    K    -1     I 

DO    135    IP    =    1     ,     IM 
DO    136    JP    =    1     ,     IM 
PDUM(IP,JP)     =     (    -1.     *    GI(IP))     *    HROW(JP) 
IF(IP.EQ.JP)     PDUM(IP,JP)     =    1.     +    PDUM(IP,JP) 
136  CONTINUE 

135  CONTINUE 

CALL    PROD(PDUM,PKKMl ,  IM, IM,IM,PI) 
C@@@@@@@@@@@<S@@@@@@@@<§@@0<§<§@@@@@<S@@<S@@@@@@@@@@@<§<i<3@@<5@@<S  <§<§@ 
C    CALCULATE    THE    PREDICTION    OF    MEASUREMENTS 
C    USE    THIS    CALL    STATEMENT    FOR    SINGLE    ARRAY    TRACKING 
C  CALL    CZHAT(IROW,XKKMl,ZHAT) 

C    USE    THIS    CALL    STATEMENT    FOR    MULTIPLE    ARRAY    TRACKING 

CALL  CZHAT3 ( I ROW , XKKM1 , ZHAT , XB , YB , ZB  > 

ZIC(IROW)  =  ZI(IROW)  +  WHTN  *  O.OOOOl 

ZDIFF(IROW)  =  ZIC(IROW)  -  ZHAT 
C  THREE  SIGMA  GATE 

P=DMAX1 (DABS (PI (1,1)) , DABS (PI (3,3) ) , DABS (PI (5,5) ) ) 

SIG=DSQRT( (P/ ( (4S60. ) **2) )  + (DABS  (COW  ( IROW  , IROW) ) ) ) 

GATE  =  3.0  *  SIG 


IF(KK.LE.4)  GO  TO  149 
IF(DABS(ZDIFF(IROW) ) .LT.GATE)  GO  TO  149 


C 


WRITE(*,147)  KK, IROW, GATE 

147  FORMAT (//,10X, 'THREE  SIGMA  GATE  HAS  BEEN  EXCEEDED' 
+       , '  AT  TIME  ',14,'  IN  ROW  ',12,'  GATE  :   ,D14.S) 

DO  14S  LGJ  =  1  ,  IM 
GI (LGJ)  =0.0 

148  CONTINUE 

C  TAG  INVALID  TIME  MEASUREMENT 

ZDIFF(IROW)  =  999. 
C 
c<  <<<<<<<<  ««<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 

cxck/k:=xck/k-i    ]+gck]*czckd 

C  -ZCK/K-133-. 

149  DO  150  I  =  1  ,  IM 

XI  (I)  =  XKKMl(I)  +  GI(I)  *  ZDIFF(IROW) 

150  CONTINUE 

C<  <<<<<<«<<<<<<<<<<<<<<<<<<««<<<«  <<<<<<<<<<<<<<<<<<<<<< 

IF(IR0W.EQ.4)     GO    TO    152 
DO    153    I    =    1     ,     IM 
XKKM1  (I)     =    XI  (I) 
153  CONTINUE 
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DO  155  I  =  1  ,  IM 
DO  154  J  =  1  ,  IM 
PKKM1 <I,J)  -  PI (I, J) 

154  CONTINUE 

155  CONTINUE 
132    CONTINUE 

C$$**-$**f*S$$*S**********f$*$*$$*$$**$$$*$^*f*Sff*****f$**f 
152    DO  156  1=1,  IM 
XKK(I)  =  XI  (I) 
XKKM1 (I)  =  XI  (I) 
C  USE  THIS  ADDITIONAL  STATEMENT  FOR  SMOOTHING 

XP6<I,KK)  =  XI (I) 
C 

DO  157  J  =  1  ,  IM 
PKK(I,J)  =  PI (I, J) 
PKKM1 (I, J)  =  PI (I, J) 
C  USE  THIS  ADDITIONAL  STATEMENT  FOR  SMOOTHING 

P5(KK,I,J>  =  PI <I,J> 
C 

157  CONTINUE 

156  CONTINUE 
C>»»>>>>>>»>>>>>>>»>>>>»>»»>>>>>>>>>>>>>>>>>>>>>>>>> 
C  PREDICTION  OF  MEASUREMENTS  BASED  ON  XCK/K3 

DO  15B  I  =  1  ,  IL 
C  EDIT  INVALID  TIME  MEASUREMENTS 

IF(ZDIFFd)  .GE.999.  )  GO  TO  159 
C  USE  THIS  CALL  STATEMENT  FOR  SINGLE  ARRAY  TRACKING 
C         CALL  CZHAT<I,XKKM1 ,ZHAT) 
C  USE  THIS  CALL  STATEMENT  FOR  MULTIPLE  ARRAY  TRACKING 

CALL  CZHAT3  < I , XKKM1 , ZHAT , XB , YB , ZB ) 

ZDIFF(I)  =  DABS(ZICd)  -  ZHAT) 

GO  TO  158 
159     ZDIFF(I)  =  0. 

NZDIFF  =  NZDIFF  -1 

158  CONTINUE 

C>>>>>>>>>>>>> >>>>>> >>>>>>>>>>>>>>>> >>>>>>>>>>>>>>>>>>>>>>> 
C  ABSOLUTE  AVERAGE  VALUE  OF  THE  DIFFERENCES  IN  MEASUREMENT 

IF(NZDIFF.EO.O)  GO  TO  160 

ZDIFTO  =  DABS(ZDIFF<1)+ZDIFF<2)+ZDIFF<3)+ZDIFF<4) ) 

ZDIFAV  =  ZDIFTO  /  NZDIFF 

IF(KK.LE.4>  GO  TO  160 

IF(MINE.EQ. 1)  SMTH(KK)  =  ZDIFAV 

IFCMINE.GT.3)  GO  TO  160 
C  USE  THIS  CONSTANT  (2.0D-6)  GATE 

IF(ZDIFAV.LT.2.0D-6)  GO  TO  160 
C 

WRITE <*, 1473)  KK 
1473   FORMAT <//,l OX, 'CONSTANT  GATE  HAS  BEEN  EXCEEDED  AT' 
+  , '  TIME  ' , 14* 

C  ((<(((((((((((((((<(<(<(((<((<<((((((((<((((<((<<((<(((((  ( 
C  USE  THESE  STATEMENT  TO  INCREASE  THE  GAIN  IN  MULTIPLE  & 


SINGLE  ARRAY  TRACKING 

SIGAAC  =  3.0  *  SIGACC 
SIGDDI  =  3.0  *  SIGDIV 
SIGCCC  =  3.0  *  SIGCC 


USE 


THIS  CALL  STATEMENT  TO  CALCULATE  ADAPTIVE  Q-MATRIX 

CALL  QF I ND  <  KK , XKK , PKK , S I GAAC , S I GDD I , S I GCCC , A 1 4 , Q ) 

CALL  ADD(PKK,Q,IM, IM,PKKM1) 

MINE  =  MINE  +  1 

GO  TO  163 
((((((((((((((((((((((((((((((((((((((((((((((((((((((((a 
160    MINE  =  1 

NZDIFF  =  4 


>01 


WRITE(7,301)  KK,(XKK(J),J  ■  1  ,  IM) 
WRITE(13,301)  KK, <PKK<I , I) , I  =  1  ,  IM) 
F0RMAT(I5,5(4X,D14.S) > 


XKERR(KK)  = 
YKERR ( KK )  = 
ZKERR(KK)  = 


XKK<1)  -  TRUX (KK) 
XKK (3)  -  TRUY(KK) 
XKK (5)  -  TRUZ(KK) 


WRITE (9,306)  KK,XKERR(KK) , YKERR (KK) ,ZKERR(KK) 
306    F0RMAT(I5,3(4X,D14.S) ) 
:  DETERMINE  MAX  &  MIN  ERRORS  AND  THE  TIME  SLOTS 

IF(KK.EQ.l)  THEN 


KX1K  = 

KK 

. 

KX2K  = 

KK 

KY1K  = 

KK 

. 

KY2K  = 

KK 

KZ1K  = 

KK 

KZ2K  = 

KK 

XI KERR 

=  XKERR(KK) 

X2KERR 

=  XKERR(KK) 

Y1KERR 

=  YKERR (KK) 

Y2KERR 

=  YKERR (KK) 

Z1KERR 

=  ZKERR (KK) 

Z2KERR 

=  ZKERR (KK) 

ENDIF 

IF(XKERR(KK) .GT.X1KERR) 

KX1K  = 

KK 

IF(XKERR(KK) .GT.X1KERR) 

XI KERR 

=  XKERR(KK) 

IF(XKERR(KK) .LT.X2KERR) 

KX2K  = 

KK 

IF(XKERR(KK) .LT. X2KERR) 

X2KERR 

=  XKERR(KK) 

I F ( YKERR ( KK ) . GT . Y 1 KERR ) 

KY1K  = 

KK 

IF (YKERR (KK) . GT. Y1KERR) 

Y1KERR 

=  YKERR (KK) 

IF (YKERR (KK) . LT. Y2KERR) 

KY2K  = 

KK 

IF (YKERR (KK) . LT. Y2KERR) 

Y2KERR 

=  YKERR (KK) 

IF(ZKERR(KK) .GT.Z1KERR) 

KZ1K  = 

KK 

IF(ZKERR(KK) .GT.Z1KERR) 

Z1KERR 

=  ZKERR (KK) 

IF(ZKERR(KK) .LT.Z2KERR) 

KZ2K  = 

KK 

I F ( ZKERR ( KK ) . LT . Z2KERR ) 

Z2KERR 

=  ZKERR (KK) 
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C) ))))))))>>>)))>))))>)))})))))))))))>)>))))))))))))>>))))  ) 
CPCK+l/K    ]=<PHI(5x5)     *    P    C    K    /    K    3(5x5)     *    PHIT(5x5)> 

C  ■*■  Q  C '  K  1 

C  USE  THIS  CALL  STATEMENT  TO  CALCULATE  ADAPTIVE  Q-MATRIX 

CALL  QF I ND ( KK , XKK , PKK , S I GACC , S I GD I V , S I GCC ,  A 1  4  ,  Q ) 

CALL  PROD(PHI , PKK , IM, IM, IM,PHIPKK) 

CALL  PROD(PHIPKK,PHIT, IM, IM, IM,PKTEMP) 

CALL  ADD(PKTEMP,Q,IM,IM,PKKM1) 
C 

CALL  MMULT(PHI,XKK,IM,IM,XKKM1) 
C)  ))))))))))))))))))))))))))))>)))))))))))))))))))))))))))  ) 
C  USE  THESE  STATEMENTS  FOR  SMOOTHING 

DO  302  IG  =  1  ,  IM 
XP(IG,KK)  =  XKK(IG) 

302  CONTINUE 

DO  303  III  =  1  ,  IM 
DO  304  JJJ  =  1  ,  IM 
SSI (KK,III,JJJ)  =  PKKM1 (III, JJJ) 
PI (KK,III ,JJJ)  =  PKK(III,JJJ) 
304     CONTINUE 

303  CONTINUE 
C########################################################## 
C  SMOOTHING  STARTS  HERE 

IF(KK.LE. JTIME)  GO  TO  123 
DO  500  K  =  1  ,  JTIME 
KI  =  JTIME  -  K  +  1 
WRITE<*,561)  KI 
561     FORMAT </,10X, ' IN  SMOOTHING  AT  TIME  : ' , 15) 
DO  501  I  =  1  ,  IM 
XP1 (I)  =  XP6(I,KI> 

501  CONTINUE 

DO  502  I  =  1  ,  IM 
DO  503  J  =  1  ,  IM 
P2 ( I , J )  =  P5 ( K I , I , J ) 
SS3(I,J)  =  SSI (KI,I,J) 
IF<KI.LE.4)  GO  TO  503 
IF(SMTH(KI) .GE.2.0D-6)  SS3 ( I , J )  =  3.6  *  SS3 ( I , J ) 

503  CONTINUE 

502  CONTINUE 

c  A(K)  =  P(K/K)  *  transpose: phi:  *  invcp(k+i/k) : 

CALL  TRANS (PHI ,IM,IM,PHIT) 

CALL  RECIP(SS3, IM,SS3R) 

CALL  PR0D(SS3,SS3R, IM, IM, IM,CH) 

CALL  PR0D(PHIT,SS3R, IM, IM, IM, TEMPI) 

CALL  PR0D(P2, TEMPI, IM,IM,IM,AK) 

C  X(K/N)  =  X(K/K)  +  A(K)  *  C  X(K+1/N)  -  X(K+1/K)  1 
DO  504  I  =  1  ,  IM 
XNNM1 (I)  -  XP(I ,KI+1) 

504  CONTINUE 
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CALL 
CALL 
CALL 
CALL 
DO  5( 


MMULT(PHI  ,  XP1  ,  IM,IM,SS2) 
SUB<XNNM1 ,SS2,IM,1 ,TEMP2) 
PR0D(AK,TEMP2, IM, IM, 1 ,TEMP3> 
ADD(XP1 ,TEMP3, IM,1 ,XKKS> 


I  =  1 


IM 


XP(I,KI>  =  XKKS(I) 
505     CONTINUE 

WRITE(S,301)  KI, (XKKS(J) ,J  =  1  ,  IM) 
XSERR(KI)  =  XKKS(l)  -  TRUX(KI) 
YSERR(KI)  =  XKKSC3)  -  TRUY(KI) 
ZSERR(KI)  =  XKKS(5)  -  TRUZ(KI) 

WRITE (10,306)  KI , XSERR(KI) ,YSERR(KI) ,ZSERR(KI) 
C  DETERMINE  MAX  &  MIN  ERRORS  AND  THE  TIME  SLOTS 
IF(K.EQ.l)  THEN 

KX1S  =  KI 

KX2S  =  KI 

KYIS  =  KI 

KY2S  =  KI 

KZ1S  =  KI 

KZ2S  =  KI 

X1SERR 

X2SERR 

Y1SERR 

Y2SERR 

Z1SERR 

Z2SERR 
END  IF 

IF(XSERR(KI) 
IF(XSERR(KI) 
IF(XSERR(KI) 
IF(XSERR(KI) 
IF<YSERR(KI) 
IF<YSERR(KI) 
IF(YSERR(KI) 
IF(YSERR(KI) 
IF(ZSERR(KI) 
IF(ZSERR(KI) 
IF(ZSERR(KI) 


XSERR(KI) 
XSERR<KI) 
YSERR(KI) 
YSERR(KI) 
ZSERR(KI) 
ZSERR(KI) 


GT.X1SERR) 
GT.X1SERR) 
LT.X2SERR) 
LT. X2SERR) 
ST. Y1SERR) 
GT.Y1SERR) 
LT.Y2SERR) 
LT.Y2SERR) 
GT. Z1SERR) 
GT. Z1SERR) 


KX1S  =  KI 

X1SERR  =  XSERR(KI) 

KX2S  =  KI 

X2SERR  =  XSERR(KI) 

KYIS  =  KI 

Y1SERR  =  YSERR(KI) 

KY2S  =  KI 

Y2SERR  =  YSERR(KI) 

KZ1S  =  KI 

Z1SERR  =  ZSERR(KI) 

KZ2S  =  KI 

Z2SERR  =  ZSERR(KI) 


LT.Z2SERR) 
IF<ZSERR(KI) .LT.Z2SERR) 

C  PCK/N)  =  P<K/K)-t-A<K)*CP<K+l/N)-P<K+l/K)  3*TRANSP0SEC  A  (K)  ] 
DO  506  I  =  1  ,  IM 
DO  507  J  =  1  ,  IM 
PNNM1(I,J)  =  P1(KI+1,I,J) 
507      CONTINUE 
506     CONTINUE 

CALL  SUB(PNNM1,SS3,IM,IM,TEMP4) 
CALL  TRANS(AK,IM,IM,AKT) 
CALL  PR0D(TEMP4,AKT, IM, IM, IM,TEMP5) 
CALL  PROD (AK, TEMPS, IM, IM, IM,TEMP6) 
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CALL  ADD(P2,TEMP6, IM, IM,PKKS) 
DO  508  1=1  ,  IM 
DO  509  J  =  1  ,  IM 
PI (KI,I,J)  =  PKKSCI ,J) 
509      CONTINUE 
508     CONTINUE 
C******$$*****$***f*S$****f*^***$^SS^.$ff***ff$**^$$^$^^$f** 
WRITE  (11, 301)  KI  ,  (PKKSd  ,  I)  ,  I  =  1  ,  IM) 

500    CONTINUE 
C########################################################## 
1 28   CONT I NUE 

WRITE (12,800) 

800  FORMAT (10X,'  TLME',4X,'   MAX.  ERROR   ' ,4X,'  TIME',4X 
+  ,  '   MIN.  ERROR   '  ) 

WRITE (12,801)  KX1K,X1KERR,KX2K,X2KERR,KY1K,Y1KERR, 
+KY2K , Y2KERR , KZ IK , Z 1KERR , KZ2K , Z2KERR 

WRITE (12,801)  KX1S,X1SERR,KX2S,X2SERR,KY1S,Y1SERR, 
+KY2S , Y2SERR , K  Z 1 S , Z 1 SERR , KZ2S , Z2SERR 

801  FORMAT (3 (/, 10X , 15 , 4X , D14. 8 , 4X , 15 , 4X , Dl 4. 8) ) 
STOP 

END 
C 

SUBROUTINE  TRANS (AA,NR ,NC,BB) 
REAL*8  AA(NR,NC) ,BB(NC,NR) 
DO  3  I  =  1  ,  NR 
DO  30  J  =  1  ,  NC 
BB(J,  I)  =  AA(I ,J) 
30     CONTINUE 

3  CONTINUE 
RETURN 
END 

C 

SUBROUT I NE  PROD ( AA , BB , NRA , NCA , NCB , CC ) 
REAL*8  AA ( NRA , NCA ) , BB ( NCA , NCB ) , CC ( NRA , NCB ) 
DO  4  I  =  1  ,  NRA 
DO  40  J  =  1  ,  NCB 
CC(I ,J)  =0.0 

40  CONTINUE 

4  CONTINUE 

DO  41  1=1  ,  NRA 
DO  410  J  =  1  ,  NCB 
DO  411  K  =  1  ,  NCA 
CC(I,J)  =  CC(I,J)  +  AA(I,K)  *  BB(K,J) 
411     CONTINUE 
410    CONTINUE 

41  CONTINUE 
RETURN 
END 

C 

SUBROUTINE  TRAJEC <KK , DATR , Z I , TD) 
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REALMS  DATR<17) ,ZI (4) ,TD<3) , COEFF , RANGE , VEL , T 
DATA  VEL/4860.0/, I  IK/3/, IIM/5/ 
T  =  0.0 

COEFF  =1.0/  VEL 

Z I ( 1 ) =COEFF*DSQRT ( ( (DATR ( 1 ) +15. 0) **2) 
+  +  (  (DATR  (2)  +15.0)**2>  +  <  < DATR  ( 3 )  + 1 5 .  O )  **2 )  ) 

ZI <2)=C0EFF*DSQRT( ( (DATR ( 1 ) -15. 0) **2) 
+  +  (  (DATR(2)+15.0)**2)+(  (DATR  (3) +15.  0)  **2)  ) 

ZI (3)=C0EFF*DSQRT( ( (DATR ( 1 ) +15. 0) **2> 
+  +( (DATR(2)-15.0)**2)+< (DATR (3) +15. 0) **2) ) 

ZI (4)=C0EFF*DSQRT( ( (DATR ( 1 ) +15. 0) **2> 
+  +( (DATR(2)+15.0)**2)+( (DATR (3) -15. O) **2) ) 

DO  5  I  =  1  ,  UK 
TD  ( I )  =  DATR  (  I.) 
5     CONTINUE 
C  USE  THIS  STATEMENT  FOR  STRAIGHT  RUN 

C       IF( (KK.LE.DATR(17) ) .AND. (KK. GT. DATR ( 16) ) )  GO  TO  50 
C  USE  THESE  STATEMENTS  FOR  MANEUVERING  RUN 
58    IF( (KK.LE.49) . AND. (KK.GT.22) )  GO  TO  50 
IF( (KK.LE.98) . AND. (KK.GT.71) )  GO  TO  50 
IF( (KK.EQ.50) .OR. (KK.EQ.99) )  THEN 
C 

C  FIRST  DATA  FOR  TRUE  TRAJECTORY  IN  SINGLE  ARRAY  TRACKING 
C        DATR (2)  =  1300.0 
C        DATR (3)  =0.0 
C 

C  SECOND  DATA  FOR  TRUE  TRAJECTORY  IN  SINGLE  ARRAY  TACKING 
DATR (2)  =  1000.0 
DATR (3)  =  300.0 
DATR (4)  =  -50.0 
DATR (5)  =0.0 
DATR(6)  =  0.0 
DATR (7)  =0.0 
DATR (8)  =0.0 
DATR (9)  =0.0 
DATR (10)  =0.0 
DATR(ll)  =0.0 
DATR (12)  =  4.712389 
DATR (13)  =  0.1745329 
DATR (14)  =0.1 
DATR(15)  =  8. 1 
END  IF 
C 
57    DATR (7)  =0.0 
DATR (8)  =0.0 
DATR (14)  =  1.31 
GO  TO  51 
50    DATR (14)  =  0.005 

53    DATR (12)  =  DATR (12)  +  DATR (13)  *  DATR (14) 
DATR ( 7 )  =  DATR (15)  *  DCOS ( DATR (12)) 
DATR(8)  =  DATR(15)  *  DSIN (DATR ( 12) ) 
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51  DO  52  I  =  1  ,  IIM 

DATR(I)  =  DATR(I)  +  DATR (1+3)  *  DATR (14) 
+  +  ( ( (DATR<14) >**2> /2)  *  DATR (1+6) 

52  CONTINUE 

T  =  T  +  DATR (14) 

IF (DABS (T  -  1.31) .LE. O.OOOl)  RETURN 
GO  TO  53 
END 
C 

SUBROUTINE  TRJC3 <KK , DATR, Z I , TD, XB , YB, ZB) 
REAL*8  DATR(17) ,ZI (4) ,TD<3) ,XB(4) ,YB(4) ,ZB(4> ,COEFF 
REALMS  VEL,T 

DATA  VEL/4860. 0/ , I  IK/3/ , I IL/4/ , I IM/5/ 
T  =  0.0 

COEFF  =  1.0  /  VEL 
DO  12  I  =  1  ,  IIL 
ZI(I)  =  COEFF  *  DSQRT( ( (DATR(l)  -  XB(I))**2) 
-•-    h-  ((DATR  (2)  -  YB(I))**2)  +  ((DATR  (3)  -  ZB(I))**2)> 
12    CONTINUE 

DO  120  I  =  1  ,  UK 
TD(I)  =  DATR (I) 

120  CONTINUE 

C  USE  THIS  STATEMENT  FOR   STRAIGHT  RUN 

IF( (KK.LE.DATR(17) ) .AND. (KK. GT. DATR ( 16) ) )  GO  TO  121 
C  USE  THESE  STATEMENTS  FOR  MANEUVERING  RUN 
C  128   IF( (KK.LE.49) .AND. (KK.GT.22) )  GO  TO  121 
C       IF( (KK.LE.98) .AND. (KK.GT.71) )  GO  TO  121 
C        IF( (KK.EQ.50) .OR. (KK.EQ.99) )  THEN 
C         DATR (2)  =  7000.0 
C        DATR (3)  =  300.0 
C         DATR (4)  =  -50.0 
C         DATR (5)  =0.0 
C         DATR (6)  =0.0 
C        DATR (7)  =0.0 
C        DATR (8)  =0.0 
C         DATR (9)  =0.0 
C        DATR (10)  =0.0 
C        DATR(ll)  =0.0 
C         DATR (12)  =  4.712389 
C         DATR(13)  =  0.1745329 
C         DATR (14)  =  O.  1 
C         DATR (15)  =  8. 1 
C       END IF 
C 
127   DATR (7)  =0.0 

DATR (8)  =0.0 

DATR(14)  =  1.31 

GO  TO  122 

121  DATR(14)  =  0.005 

124   DATR (12)  =  DATR (12)  +  DATR (13)  *  DATR (14) 
DATR (7)  =  DATR (15)  *  DCOS ( DATR ( 12) ) 
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1 22 


123 


DATR(8> 
DO  123  I 
DATR(I) 


=  DATR(15) 
=  1  ,  IIM 
=  DATR(I) 


*  DSIN(DATR(12) ) 

+  DATR(I+3)  *  DATR(14) 
+  < < <DATR(14) )**2) /2) 


*  DATR(I+6) 


CONTINUE 

T  =  T  +  DATR(14) 

IF(DABS(T  -  1.31) 

GO  TO  124 

END 


LE. O.OOOl)  RETURN 


SUBROUTINE  CHROW ( I ROW, XKKM1 ,HROW) 

REAL*8  HR0W(5) , XKKM1 (5) , COEFF , DENOM , DENOM 1 , DEN0M2 

REAL*8  VEL , A 1 , A2 , A3 , DEN0M3 , DEN0M4 

DATA  VEL/ 4860.0./ 

COEFF  =  1.0  /  VEL 

DENOM 1=DSQRT( ( (XKKM1 ( 1 ) +15. O) **2) + ( (XKKM1 (3) +15.0) **2) 
+  +< (XKKM1 <5)+15.0)**2) ) 

DEN0M2=DSQRT ( ( (XKKM1 ( 1 ) -15. 0) **2) + ( (XKKM1 <3) +15.  0)  **2 ) 
+  +( (XKKM1 <5)+15.0)**2) ) 

DEN0M3=DSQRT ( < (XKKM1 ( 1 ) +15. 0) **2) + ( (XKKM1 (3) -15. 0) **2) 
+  +< (XKKM1 (5)+15.0)**2) ) 

DEN0M4=DSQRT ( ( (XKKM1 ( 1 > +15. 0) **2) + ( (XKKM1 (3) +15. 0) **2) 
+  +< <XKKM1 <5)-15.0)**2) > 

Al  =  1.0 

A2  =  1.0 

A3  =  1.0 

DENOM  =  DENOM 1 

IFCIR0W.E0.2)  DENOM  = 

IF(IR0W.EQ.3)  DENOM  = 

IF(IROW.E0.4)  DENOM  « 

IF(IR0W.EQ.2)  Al  =  - 


HROW ( 1 ) 
IFdROW. 
HROW (3) 
IFdROW. 
HROW (5) 
HROW (2) 
HROW  <  4 ) 
RETURN 
END 


DEN0M2 
DEN0M3 
DEN0M4 
.0 

=  COEFF  *  ( (XKKM1 (1) 
EQ.3)  A2  =  -  1.0 
=  COEFF  *  ( (XKKM1 <3) 
EQ.4)  A3  =  -  1.0 
=  COEFF  *  ( (XKKM1 (5) 
=  0.0 
=  0.0 


+  Al  *  15.0)  /  DENOM) 


+  A2  *  15.0)  /  DENOM) 


+  A3  *  15.0)  /  DENOM) 


SUBROUTINE  CHR0W3 ( IROW, XKKM1 ,HROW , XB , YB , ZB) 

REAL*8  HROW (5) , XKKM1 (5) , COEFF , DENOM , VEL , XB < 4) ,YB(4) 

REAL*8  X0,Y0,Z0,ZB(4) 

DATA  VEL/4860.0/ 

COEFF  =  1.0  /  VEL 

XO  =  XB(IROW) 

YO  =  YB(IROW) 

ZO  =  ZB(IROW) 

DENOM  =  DSQRT( ( (XKKM1 ( 1 ) -XO) **2) + ( (XKKM1 (3)-Y0)**2) 

+  ( (XKKM1 (5)-Z0)**2) ) 
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HROW ( 1 ) 
HROW<2) 
HROW (3) 
HROW (4) 
HROW (5) 
RETURN 
END 


COEFF  *  ( (XKKM1 (1) 

0.0 

COEFF  *  ( (XKKM1 (3) 

0 .  0 

COEFF  *  ( (XKKM1 (5) 


XO)  /  DENOM) 
YO)  /  DENOM) 
ZO)  /  DENOM) 


SUBROUT I NE  MMULT ( AA , BB , NRA , NCA , CC ) 
REAL*8  AA  <  NRA , NCA )  ,  BB  ( NCA )  ,  CC  <  NRA ) 
DO  6  I  =  1  ,  NRA 
CCCI)  =  0.0 
DO  60  J  =  1  ',  NCA 
CC(I)  =  CC(I)  +  AA<I,J)  *  BB(J) 
60     CONTINUE 

6  CONTINUE 
RETURN 
END 

SUBROUTINE  VMULT ( AA , BB ,NE,CC> 
REAL*8  AA(NE) ,BB<NE) ,  CC 
CC  =  0.0 
DO  7  I  =  1  ,  NE 
CC  =  CC  +  AA(I)  *  BB<I) 

7  CONTINUE 
RETURN 
END 

SUBROUT I NE  CZHAT ( I ROW , XKKM 1 , ZHAT ) 

REAL*8  XKKM1 (5) , ZHAT, COEFF, VEL 

DATA  VEL/4860.0/ 

COEFF  =  1.0  /  VEL 

IFCIROW.EQ.  1)  ZHAT=COEFF*DSQRT ( <  (XKKM1  ( 1 ) +15. O) **2)  ■ 
+       ( (XKKM1 <3)+15.0)**2)+< (XKKM1 <5) +15. O) **2) ) 

IF(IR0W.E0.2)  ZHAT=COEFF*DSQRT ( (  (XKKM1 < 1 ) -15. 0) **2) - 
+       (  (XKKM1 <3)+15.0)**2>+< (XKKM1 (5) +15. O) **2)  ) 

IF(IR0W.EQ.3)  ZHAT=COEFF*DSQRT < ( (XKKM1 ( 1 ) +15. 0) **2) - 
+       < (XKKM1 <3)-15.0>**2)+< (XKKM1 <5) +15. O) **2) ) 

IF(IR0W.EQ.4)  ZHAT=COEFF*DSQRT ( (  (XKKM1 < 1 ) +15. 0) **2)  - 
+        < (XKKM1 <3)+15.0)**2)+< (XKKM1 (5) -15. O ) **2) ) 

RETURN 

END 

SUBROUTINE  CZHAT3 ( IROW , XKKM1 , ZHAT , XB , YB , ZB) 

REAL*8  XKKM1 (5) , ZHAT , COEFF , VEL , XB <4) ,YB(4) ,ZB(4) , XO 

REAL*8  YO,ZO 

DATA  VEL/4860.0/ 

COEFF  =  1.0  /  VEL 

XO  =  XB<IROW) 

YO  =  YB(IROW) 

ZO  =  ZB(IROW) 
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ZHAT  =  COEFF  *  DSQRT < ( ( XKKM1 ( 1 )  -  X0>**2)  + 
+  <<XKKM1(3>  -  Y0)**2)  +  C (XKKM1 <5)  -  Z0)**2)) 

RETURN 
END 

SUBROUTINE  NOISE <R,P) 

REALMS  Y(6) ,X(6> ,S(S) ,R,P,BB,P1 

DATA  Y/O.O, .0228, .0668,. 1357, .2743, .5/ 

DATA  X/-3.01 ,-2.0,-1.5,-1.0,-0.6,0.0/ 

DATA  S/43. 8596, 11. 3636, 7. 25689, 2. 891352, 2. 65887/ 

BB  =  1.0 

PI  =  R  *  317.0 

R  =  DM0D(P1  ,  BB) 

P  =  R 

1  =  1 

IF(P.ST.0.5)  P  =  1.0  -  R 

8  IF(P.LT.  Y(I-t-l)  )  GO  TO  80 
1  =  1  +  1 

GO  TO  8 
80    P  =  <<P  -  Y(D)  *  S(I)  +  X(I>> 
IF(R.GE.0.5)  P  =  -  P 
RETURN 
END 

ft 

SUBROUTINE  ADD < AA, BB,NR, NC , CO 
REAL*8  AA(NR,NC) ,BB(NR,NC) ,CC(NR,NC) 
DO  9  I  =  1  ,  NR 
DO  90  J  =  1  ,  NC 
CC ( I , J )  =  AA  < I , J )  +  BB ( I , J ) 
90     CONTINUE 

9  CONTINUE 
RETURN 
END 

SUBROUT I NE  QF I ND ( KK , XKK , PKK , S I GACC , S I GD I V , S I GCC , A , Q ) 

REAL*8  XKK<5) ,PKK(5,5) ,Q(5,5) , SIGACC , SIGDI V , SIGCC , A 

REAL*8  A2,A3,B,C,D,E1  ,E12,E2,G1  ,  G2,  G3,  SIGAACSIGDDI 

REAL*8  SIGCCC,A1 

INTEGER  KK 

IF(KK.NE. 1)  GO  TO  111 

DO  11  1=1  ,5 
DO  110  J  =  1  ,  5 
Q(I,J)  =0.0 
110    CONTINUE 
11    CONTINUE 

SIGACC  =  SIGACC  **2 

Q(5,5)  =  (SIGDIV  **2)  *  (A  **2) 

SIGCC  =  SIGCC  **2 

Gl  =  (  A  **2    )  /  2.0 

G2  =  Gl  **2 

G3  =  A  *  Gl 
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A2  =  A 

**2 

Ill 

Al  =  XKK<2)  **2  +  XKK(4)  **2 

A3  =  XKK 

(2)  /  DSQRT(Al) 

B  =  XK* 

<<4) 

C  =  XKK<4)  /  DSQRT(Al) 

D  =  XKK<2) 

El  =  ( 

A3  **2>  *  SIGACC  +  (  B 

E12  =  1 

^3 

*  C  *  SIGACC  -  B  *  D 

E2  =  ( 

C 

**2  )  *  SIGACC  +  (  D 

Q(l,l> 

= 

El  *  G2 

Q<1,2> 

= 

G3  *  El 

Q<1,3) 

= 

E12  *  G2 

Q<1,4) 

= 

G3  *  E12 

Q<2,2> 

s 

A2  *  El 

Q<2,3) 

= 

G3  *  E12 

0(2,4) 

= 

A2  *  El 2 

Q<3,3) 

= 

G2  *  E2 

0(3,4) 

= 

G3  *  E2 

Q<4,4> 

= 

A2  *  E2 

DO  112 

I 

=  1,4 

DO  113  J  =  1  ,  I 

Q(I ,J) 

=  Q(J, I) 

113 

CONTINUE 

112 

CONTINUE 

RETURN 

END 

**2  )  *  SIGCC 

*  SIGCC 

**2  )  *  SIGCC 


SUBROUTINE  SUB ( AA,BB,NR ,NC,CC) 
REAL*8  AA(NR,NC) ,BB<NR,NC) ,CC(NR,NC) 
DO  12  I  =  1  ,  NR 
DO  120  J  =  1  ,  NC 
CC(I,J)  =  AA(I,J)  -  BBC  I, J) 
120    CONTINUE 
12    CONTINUE 
RETURN 
END 

SUBROUTINE  RECIP ( AA, NN ,CC) 

REAL*8  AA(NN,NN) ,DD(5,10) ,CC(NN,NN) 

DO  14  K  =  1  ,  NN 


DO  140  J  =  1  ,  NN 

DD(K,J)  =  AA(K,J) 

140 

CONTINUE 

14 

CONTINUE 

DO  141  K  =  1  ,  NN 

I  =  K  +  NN 

DO  142   J  =  6  ,  10 

IF(I.NE.J)  GO  TO  143 

DD<K,J)  =  1. 

GO  TO  142 

143 

DD(K,J)  =  0. 

io: 


142    CONTINUE 
141   CONTINUE 

DO  144  K  =  1  ,  NN 
M  -  K  +  1 
DO  145  J  =  M  ,  10 
DD(K,J)  =  DD<K,J)  /  DD<K,K) 

145  CONTINUE 
DD(K,K)  =  1. 

DO  146  L  =  1  ,  NN 
IF(L.EO.K)  GO  TO  146 
DO  147  I  =  1  ,  10 
IF(I.EQ.K)  GO  TO  147 
DD<L,I)  =  DD<L,I>  -  DD(L,K)  *  DD(K,I) 

147  CONTINUE 
DD<L,K)  =  O.  •' 

146  CONTINUE 
144   CONTINUE 

DO  148  K  =  1  ,  NN 
DO  149  J  =  1  ,  NN 
I  =  J  +  NN 
CC(K,J)  =  DD<K,I) 
149    CONTINUE 

148  CONTINUE 
RETURN 
END 
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APPENDIX  C 
PLOTTING  PROGRAM  LISTING  FOR  HP  PLOTTER 


SST0RAGE:2 
$DEBUG 
$NOLIST 
C 

PROGRAM  PLOTTER 
C 

CHARACTER*40  TITLE 

CHARACTER*35  LEGEND, SUBTITLE 

CHARACTER*25  NAMEX , NAMEY 

REAL  X(245) ,Y(245) 

REAL  0(245) ,P<245) ,R<245) ,S(245) ,T(245) ,U(245> 

INTEGER*2  IC 

DATA  IC/O/ 
C 

C  USE  THESE  FOR  MULTIPLE  ARRAY  TRACKING 
C       TITLE  = 'MULTIPLE  ARRAY  ADAPTIVE  MANEUVERING  RUN ' 

TITLE  =' MULTIPLE   ARRAY   ADAPTIVE   STRAIGHT  RUN ' 
C 

C  USE  THESE  FOR  SINGLE  ARRAY  TRACKING 

C       TITLE  =  SINGLE  ARRAY   ADAPTIVE   MANEUVERING  RUN ' 
C       TITLE  = 'SINGLE   ARRAY    ADAPTIVE   STRAIGHT   RUN' 

OPEN  <  5 , F I LE= ' XKK . DAT ' , ST ATUS= ' OLD ' > 

DO  32  LENG  =  1  ,  241 
READ(5,*,END=33)  O(LENG) ,P(LENG) ,R(LENG) ,S(LENG) , 
+  T(LENG) ,U(LENG) 

32  CONTINUE 

33  CONTINUE 

LENG  =  LENG  -  1 

CLOSE ( 5 , STATUS= ' KEEP  * ) 

NAMEX  ='XCK/KD   (FT)  * 

NAMEY  ='YCK/K3   (FT) ' 

SUBTITLE  =  ' 

LEGEND  = 'FILTERED  ESTIMATE  OF  TRAJECTORY' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , P , S , LENG , 
+  SUBTITLE) 

OPEN ( 5 , F I LE= ' PKK . DAT ' , ST ATUS= ' OLD ' ) 

DO  34  LENG  =  1  ,  241 

READ(5,*,END=35)  0 (LENG) ,P(LENG) ,R(LENG) ,S(LENG) , 
+  T(LENG) ,U(LENG) 

34  CONTINUE 

35  CONTINUE 

LENG  =  LENG  -  1 
CLOSE ( 5 , STATUS= ' KEEP ' ) 
NAMEX  ='TIME   SLOTS' 
NAMEY  =' (FT**2) ' 
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SUBTITLE  =*PCK/K3 (1,1) ' 

LEGEND  =' FILTERED  ERROR  COVARIANCE  P(K/K)' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , P , LENG , 

*  SUBTITLE) 
NAMEY  = ' ( (FT/SEC) **2) ' 

SUBTITLE  ='PCK/K: (2,2) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , R , LENG , 
+  SUBTITLE) 

NAMEY  =' (FT**2) ' 

SUBTITLE  ='PCK/K: (3,3) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , S , LENG , 
+  SUBTITLE) 

NAMEY  = ' ( (FT /SEC) **2) ' 

SUBTITLE  ='PCK/K3 (4,4) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , O , T , LENG , 
+  SUBTITLE) 

NAMEY  =' (FT**2) ' 

SUBTITLE  ='PCK/K3 (5,5) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , U , LENG , 

♦  SUBTITLE) 
OPEN ( 5 , F I LE= ' XKERR . DAT ' , STATUS= ' OLD ' ) 

DO  38  LENG  =  1  ,  241 

READ(5,*,END=39)  O(LENG) ,P(LENG) ,R(LENG) ,S(LENG) 

38  CONTINUE 

39  CONTINUE 

LENG  =  LENG  -  1 

CLOSE ( 5 , STATUS= ' KEEP ' ) 

NAMEY  =  'X  ERROR   (FT) ' 

SUBTITLE  =  ' 

LEGEND  =' ERROR  IN  FILTERED  ESTIMATE' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , P , LENG , 
+  SUBTITLE) 

NAMEY  ='Y  ERROR   (FT)  * 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , O , R , LENG , 
«■  SUBTITLE) 

NAMEY  =  'Z  ERROR   (FT) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , O , S , LENG , 


+                                         SUBTITLE) 

OPEN (5,FILE='XKN. DAT* ,STATUS= ' OLD ' ) 

DO  40  LENG  =  1  ,  241 

READ ( 5 , * , END=4 1 )  0 ( LENG ) , P ( LENG ) , R ( LENG ) , S ( LENG ) 

«■                    T(LENG)  ,U(LENG) 

40 

CONTINUE 

41 

CONTINUE 

LENG  =  LENG  -  1 
CLOSE ( 5 , STATUS= ' KEEP ' ) 
NAMEX  ='XCK/N]    (FT) ' 
NAMEY  ='YCK/N3   (FT) ' 

LEGEND  =* SMOOTHED  ESTIMATE  OF  TRAJECTORY' 
CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , P , S , LENG 

SUBTITLE) 


io; 


OPEN  <  5 , F I LE= ' PKN . DAT '  , ST ATUS= ' OLD ' ) 
DO  42  LENG  =  1  ,  241 

READ(5,*,END=43)  O(LENG) ,P(LENG> ,R(LENG) ,S(LENG) , 
+  •     T(LENG) ,U(LENG) 

42  CONTINUE 

43  CONTINUE 

LENG  =  LENG  -  1 

CLOSE ( 5 , STATUS= ' KEEP ' ) 

NAMEX  -'TIME   SLOTS' 

NAMEY  =' (FT**2) ' 

SUBTITLE  ='PCK/ND (1 ,1) ' 

LEGEND  =' SMOOTHED  ERROR  COVARIANCE  P(K/N) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND ,0,P, LENG , 
+  .  SUBTITLE) 

NAMEY  =' ( (FT/SEC) **2>  ' 

SUBTITLE  ='PCK/N] (2,2) ' 

CALL  DRAWER  <  T I TLE , NAMEX , NAMEY , LEGEND , 0 , R , LENG , 
+  SUBTITLE) 

NAMEY  =' (FT**2) ' 

SUBTITLE  ='PCK/N3 (3,3) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND ,  0  ,  S  , LENG , 
+  SUBTITLE) 

NAMEY  ='( (FT/SEC) **2) ' 

SUBTITLE  ='PCK/N: (4,4) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , O , T , LENG , 
+  SUBTITLE) 

NAMEY  =' (FT**2) ' 

SUBTITLE.  ='PCK/N: (5,5) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , U , LENG , 
+  SUBTITLE) 

OPEN (5,FILE=' XSERR.DAT' ,STATUS= 'OLD ' ) 

DO  44  LENG  =  1  ,  241 

READ(5,*,END=45)  O(LENG) ,P(LENG) ,R(LENG) ,S(LENG) 

44  CONTINUE 

45  CONTINUE 

LENG  =  LENG  -  1 

CLOSE ( 5 , STATUS= ' KEEP ' ) 

NAMEY  ='X  ERROR   (FT) ' 

SUBTITLE  =' 

LEGEND  =' ERROR  IN  SMOOTHED  ESTIMATE' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , P , LENG , 
+  SUBTITLE) 

NAMEY  ='Y  ERROR   (FT) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , R , LENG , 
+  SUBTITLE) 

NAMEY  ='Z  ERROR   (FT) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , O , S , LENG , 
+  SUBTITLE) 

STOP 

END 

SUBROUTINE  DRAWER (TITLE , NAMEX , NAMEY , LEGEND , X , Y , 
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LENG, SUBTITLE) 


CHARACTER*40  TITLE 
CHARACTER*35  LEGEND , SUBTITLE 
CHARACTER*25  NAMEX,NAMEY 
REAL  X(245) ,Y(245) 
INTEGER*2  IC 
DATA  IC/O/ 


C 
C 

C 

c 
c 


CALL  ED 

CALL  CUP < 1,0) 

CALL   PLOTS (0,9600,30) 

CALL  SYMBOL ( 2 . 0 , 6 . 65 , . 20 , T I TLE , O . 0 , 40 ) 

CALL  SYMBOL (2. 0,6. 25,. 175, LEGEND ,0. O , 35) 

USE  THIS  FOR  NOISELESS  TRACKING 

CALL  SYMB0L(6.S4,6. 25,. 175, 'WITHOUT  NOISE ', 0. 0 , 13) 


USE  THIS  FOR  NOISY  TRACKING 

CALL  SYMBOL ( 6. 84, 6. 25,. 175, ' 


WITH   NOISE' ,0.0,13) 


CALL  SYMBOL ( 1 . 60 , 2 . 45 , . 20 , SUBT I TLE , 90 . O , 35 ) 

CALL  PLOT (1.00, 1.00,-3) 

CALL  PLOTO.0,0.0,3) 

CALL  PL0T(S.0,6.0,2) 

CALL  PL0T(0.0,6.0,2) 

CALL  PLOT (0.0,0. 0,2) 

CALL  PL0T(S.0,0.0,2) 

CALL  SCALE (X, 6. 00, LENG, 1) 

CALL  SCALE ( Y, 3. 00, LENG, 1) 

CALL  STAXIS(. 180 , . 20 , . 15, . 1 12 , -1 ) 

CALL  AXIS(1.5, 1.5,NAMEX,-13,6.00,00. ,X(LENG+1> , 
+■  X(LENG+2)) 

CALL  STAXIS(. 15, .20,. Ill ,. 112,2) 

CALL  AX  IS (1.5, 1.5,NAMEY, 13,3.00,90.  ,Y(LENG+1)  , 
+  Y(LENG+2)) 

CALL  PLOT (1.50, 1.50,-3) 

CALL  LINE(X, Y, LENG, 1 ,0,3) 

CALL  PLOT (0.0,0. 0,999) 

RETURN 

END 

SUBROUTINE  ED 

CHARACTERS  C1,C2,C3,C4 

INTEGER*2  IC(4) 

EQUIVALENCE  (CI , IC ( 1 ) ) , (C2, IC (2) ) , (C3 , IC ( 
*■  (C4,IC(4)) 

DATA  IC/16#1B,16#5B, 16#32, 16#4A/ 

WRITE (*,1)  C1,C2,C3,C4 

FORMAT (1X,4A1) 

RETURN 

END 

SUBROUTINE  CUP(N,M) 


;) ) 
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CHARACTER* 1  CI , C2 , C5 , CS , LC (5) 
CHARACTER*5  CBUFF 
INTEGER*2  IC(4) 

EQUIVALENCE  (CI ,IC(1>  > , (C2, IC<2) ) , <C5, IC<3> ) , 
-t-  (C8,IC(4)  ),  (CBUFF,  LC  <  1 )  ) 

DATA  IC/16#1B, 16#5B,16#3B,16#66/ 
L= 1 0000+ 1 00*N+M 
WRITE(CBUFF,2)L 
FORMAT (15) 

WRITE(*,1)  C1,C2,LC(2) ,LC(3) ,C5,LC(4) ,LC(5> ,C8 
FORMAT (1X,8A1,\> 
RETURN 
END 
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APPENDIX  D 
PLOTTING  PROGRAM  LISTING  FOR  MONITOR 


*ST0RAGE:2 
■fDEBUG 
SNOLIST 
C 

PROGRAM  MONITOR 
C 

CHARACTER*40  TITLE 

CHARACTER*35  LEGEND 

CHARACTER*25  NAMEX , NAMEY 

REAL  X (245) ,Y(245) 

REAL  0(245) ,P<245) ,R(245) ,S(245) ,T(245) ,U(245) 

INTEGER*2  IC 

DATA  IC/O/ 
C 

C  USE  THESE  FOR  MULTIPLE  ARRAY  TRACKING 
C       TITLE  =' MULTIPLE  ARRAY  ADAPTIVE  MANEUVERING  RUN ' 

TITLE  = 'MULTIPLE   ARRAY   ADAPTIVE   STRAIGHT  RUN' 
C 

C  USE  THESE  FOR  SINGLE  ARRAY  TRACKING 

C       TITLE  = 'SINGLE  ARRAY   ADAPTIVE   MANEUVERING  RUN ' 
C       TITLE  = 'SINGLE   ARRAY    ADAPTIVE   STRAIGHT   RUN' 

OPEN ( 5 , F I LE= ' XKK . DAT  * , STATUS= ' OLD ' ) 

DO  32  LENG  =  1  ,  241 
READ(5,*,END=33)  O(LENG) ,P(LENG) ,R(LENG) ,S(LENG) , 
+  T (LENG) ,U (LENG) 

32  CONTINUE 

33  CONTINUE 

LENG  =  LENG  -  1 
CLOSE ( 5 , STATUS= ' KEEP ' ) 
NAMEX  ='XCK/KD   (FT) ' 
NAMEY  ='YCK/K:   (FT) ' 

LEGEND  = 'FILTERED  ESTIMATE  OF  TRAJECTORY' 
CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , P , S , LENG ) 
OPEN ( 5 , F I LE= ' PKK . DAT ' , STATUS= ' OLD ' ) 
DO  34  LENG  =  1  ,  241 

READ(5,*,END=35)  O(LENG) ,P(LENG) ,R(LENG) ,S(LENG) , 
+  T (LENG) 7U (LENG) 

34  CONTINUE 

35  CONTINUE 

LENG  =  LENG  -  1 

CLOSE ( 5 , STATUS= ' KEEP ' ) 

NAMEX  = ' T I ME   SLOTS ' 

NAMEY  ='PCK/KD (1,1) ' 

LEGEND  = 'FILTERED  ERROR  COVARIANCE  P(K/K) ' 

CALL  DRAWER ( T I TLE , NAME X , NAMEY , LEGEND , 0 , P , LENG ) 
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NAMEY  ='PCK/K1 (2,2) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND ,  0  ,  R  , LENG ) 
NAMEY  ='PCK/K1 (3,3) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , S , LENG ) 
NAMEY  ='PCK/K: (4,4) ' 

CALL  DRAWER  ( T I  TLE  ,  NAMEX  ,  NAMEY  ,  LEGEND  ,  0 ,  T  ,  LENG ) 
NAMEY  ='PCK/K3 (5,5) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , U , LENG ) 
OPEN <5,FILE='XKERR. DAT' ,STATUS= ' OLD ' ) 
DO  38  LENG  =  1  ,  241 

READ(5,*,END=39)  O(LENG) ,P(LENG) ,R(LENG) ,S(LENG) 
33     CONTINUE 

39  CONTINUE 

LENG  =  LENG  -  1. 
CLOSE  ( 5 ,  STATUS=. '  KEEP  '  ) 
NAMEY  =  'X  ERROR   (FT) ' 

LEGEND  =' ERROR  IN  FILTERED  ESTIMATE' 
CALL  DRAWER ( T I TLE , NAME  X , NAMEY , LEGEND ,  0  ,  P  , LENG ) 
NAMEY  ='Y  ERROR   (FT)' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , R , LENG ) 
NAMEY  ='Z  ERROR   (FT) ' 

CALL  DRAWER ( T I TLE , NAME  X , NAMEY , LEGEND , 0 , S , LENG , 
OPEN ( 5 , F I LE= ' XKN . DAT ' , STATUS= ' OLD ' ) 
DO  40  LENG  =  1  ,  241 

READ(5,*,END=41)  O(LENG) ,P(LENG) ,R(LENG) ,S(LENG) , 
+  T (LENG) ,U (LENG) 

40  CONTINUE 

41  CONTINUE 

LENG  =  LENG  -  1 
CLOSE ( 5 , STATUS=  *  KEEP  * ) 
NAMEX  ='XCK/N:   (FT) ' 
NAMEY  ='YCK/N:   (FT)' 

LEGEND  =' SMOOTHED  ESTIMATE  OF  TRAJECTORY' 
CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , P , S , LENG ) 
OPEN ( 5 , F I LE= ' PKN . DAT ' , ST ATUS= ' OLD ' ) 
DO  42  LENG  =  1  ,  241 

READ(5,*,END=43)  O(LENG) ,P(LENG) ,R(LENG) ,S(LENG) , 
+  T(LENG) ,U(LENG) 

42  CONTINUE 

43  CONTINUE 

LENG  =  LENG  -  1 

CLOSE ( 5 , STATUS= ' KEEP ' ) 

NAMEX  ='TIME   SLOTS' 

NAMEY  ='PCK/N3 (1,1) ' 

LEGEND  =' SMOOTHED  ERROR  COVARIANCE  P(K/N) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , P , LENG ) 

NAMEY  ='PCK/N: (2,2)  ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , O , R , LENG ) 

NAMEY  ='PCK/N] (3,3) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , O , S , LENG ) 

NAMEY  ='PCK/N3 (4,4) ' 
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CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND ,  0  ,  T  , LENG ) 

NAMEY  ='PCK/N3 (5,5) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , U , LENG ) 

OPEN (5, FILE-' XSERR.DAT' , STATUS- ' OLD ' ) 

DO  44  LENG  =  1  ,  241 

READ(5,*,END=45)  O(LENG) ,P(LENG) ,R(LENG) ,S(LENG) 

44  CONTINUE 

45  CONTINUE 

LENG  =  LENG  -  1 

CLOSE ( 5 , STATUS- ' KEEP ' ) 

NAMEY  ='X  ERROR   (FT) ' 

LEGEND  -'ERROR  IN  SMOOTHED  ESTIMATE' 
.  CALL  DRAWER (TITLE, NAMEX, NAMEY, LEGEND, 0,P, LENG) 

NAMEY  ='Y  ERROR   (FT) ' 

CALL  DRAWER ( T I TLE , NAME X , NAMEY , LEGEND , 0 , R , LENG ) 

NAMEY  ='Z  ERROR   (FT)' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , S , LENG) 

STOP 

END 

SUBROUT I NE  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , X , Y , LENG ) 

CHARACTER*40  TITLE 

CHARACTER*35  LEGEND 

CHARACTER*25  NAMEX, NAMEY 

REAL  X (245) ,Y(245) 

INTEGER*2  IC 

DATA  IC/O/ 
C 

CALL  ED 

CALL  CUP (1,0) 

CALL   PLOTS (0,99,99) 

CALL  SYMBOL (0.5,5. 15, . 20 , TITLE , 0. O , 40) 

CALL  SYMBOL ( 1.04,4.75, . 175 , LEGEND , O. 0 , 35) 
C 

C  USE  THIS  FOR  NOISELESS  TRACKING 

C       CALL  SYMB0L(5.38,4.75, . 175, 'WITHOUT  NOISE ', O. 0 , 13) 
C 
C  USE  THIS  FOR  NOISY  TRACKING 

CALL  SYMBOL (5. 38,4. 75, . 175, '   WITH   NOISE ', 0. 0 , 13) 
C 

CALL  PLOT (1.00, 1.00,-3) 

CALL  SCALE (X, 6. 00, LENG, 1) 

CALL  SCALE (Y, 3. 00, LENG, 1) 

CALL  STAXIS( . ISO , . 20 , . 15 , . 1 12 , -1 ) 

CALL  AXIS(0. ,0. , NAMEX, -13, 6. 00, 00. ,X (LENG+1 ) 
+  X (LENG+2) ) 

CALL  STAXIS(. 15, .20, . Ill , . 1 12,2) 

CALL  AXIS(0. ,0. , NAMEY, 13,3.00,90. ,Y (LENG+1) , 
+  Y (LENG+2)) 

CALL  LINE (X,Y, LENG, 1 ,0,3) 

CALL  PLOT (0.0,0. 0,999) 

RETURN 
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END 

SUBROUTINE  ED 

CHARACTER* 1  C1,C2,C3,C4 

INTEGER*2  IC(4) 

EQUIVALENCE  <C1 , IC ( 1 ) > , <C2 , IC (2) > , <C3, IC (3) ) , 

(C4,IC<4)) 
DATA  I C/ 1 6# 1 B , 1 6#5B , 1 6#32 , 1 6#4A/ 
WRITE(*,1>  C1,C2,C3,C4 
FORMAT <1X,4A1) 
RETURN 
END 

SUBROUTINE  CUP(N,M> 
CHARACTER* 1  CI , C2 , C5 , C8 , LC (5) 
CHARACTER*5  CBUFF 
INTEGER*2  IC(4) 
EQUIVALENCE  (CI , IC ( 1 ) ) , (C2, IC (2) ) , (C5, IC (3) ) , 

<CS,IC(4> ) , (CBUFF,LC<1) ) 
DATA  I C/ 1 6# 1 B , 1 6#5B , 1 6#3B , 1 6#66/ 
L=  1 OGOO+ 1 00*N-f-M 
WRITE(CBUFF,2)L 
FORMAT (15) 

WRITE**, 1)  C1,C2,LC(2) ,LC<3) ,C5,LC(4) ,LC(5) , CS 
FORMAT ( IX, SA1 ,  \) 
RETURN 
END 
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APPENDIX  E 
BATCH  FILES 

A.  LISTING  OF  AUTOEXEC.BAT  FILE  ON  OPERATING  SYSTEM  DISK 
ECHO  OFF 

GRAPHICS 

TIMER/S 

COPY  A: RUN. BAT  C: 

COPY  A: KEDIT.EXE  C: /V 

COPY  A: PROFILE. KED  C: /V 

C: 

RUN 

B.  LISTING  OF  RUN. BAT  FILE  ON  VIRTUAL  DISK(C) 

ECHO.  Insert  the  disk,  which  has  the  source  -file  of  the 

ECHO,  sequential  extended  Kalman  filter  and  Smoothing, 

ECHO,  into  drive  A 

PAUSE 

COPY  A: THESIS. FOR  C: 

KEDIT  C: THESIS. FOR 

COPY  C: THESIS. FOR  A: 

ERASE  C: KEDIT- EXE 

ERASE  C:  PROFILE.  K.ED 

ECHO.  Insert  the  disk,  which  has  PROFORT.EXE  and 

ECHO.  LINK. EXE,  into  drive  A,  and  the  disk,  which  has 

ECHO.  PROFORT.LIB  into  drive  B. 

PAUSE 

A:PROFORT  THESIS  /L  /E 

A:LINK  THESIS, , NULL, PROFORT 

ERASE  C: THESIS. FOR 

ERASE  C: THESIS. OBJ 

THESIS 

ECHO.  Insert  the  disk,  which  has  the  source  file  of  the 

ECHO,  sequential  extended  Kalman  filter  and  Smoothing, 

ECHO,  into  drive  A,  and  the  disk  labeled  "DATA"  into 

ECHO,  drive  B. 

PAUSE 

COPY  C: THESIS. EXE  A: 

COPY  C:*.DAT  B: 

ERASE  C:*.* 

ECHO.  Insert  the  operating  system  disk  into  drive  A,  and 

ECHO,  the  disk,  which  has  the  plotting  routine  source 

ECHO,  file  into  drive  B. 

PAUSE 

COPY  AiKEDIT.EXE  C: 

COPY  A: PROFILE. KED  C: 

COPY  B: GRAPH. FOR  C: 

KEDIT  C: GRAPH. FOR 

COPY  C: GRAPH. FOR  B: 
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ERASE  C:KEDIT.EXE 

ERASE  C: PROFILE. KED 

ECHO-  Insert  the  disk,  which  has  FORI. EXE  and  PAS2.EXE 

ECHO,  into  drive  A,  and  the  disk,  which  has  PLOTSS.LIB 

ECHO,  into  drive  B. 

PAUSE 

A: FORI  GRAPH; 

A:PAS2 

ECHO.  Insert  the  disk,  which  has  FORTRAN. LIB,  MATH. LIB 

ECHO,  and  LINK. EXE  into  drive  A. 

PAUSE 

A : L I NK  GRAPH , , NULL , B : PL0TT88+A : FORTRAN+A : MATH 

ECHO.  Insert  the  disk,  which  has  the  platting  source 

ECHO,  -file  into  drive  A  and  the  data  disk  into  drive  B. 

PAUSE 

COPY  C: GRAPH. EXE  A: 

ERASE  C:  GRAPH.  FOR 

ERASE  C: GRAPH. OBJ 

COPY  B:*.DAT  C: 

GRAPH 
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